From 39032d0560ab0a5ddfd359656ae5eb47537c6b89 Mon Sep 17 00:00:00 2001 From: Buzz Date: Sat, 29 Jul 2017 18:00:15 +1000 Subject: [PATCH] Initial commit, based on .tar.gz file as provided by 3DR , see SOURCE file --- .clang-format | 19 + .gitignore | 14 + COPYRIGHT-3DR | 37 + LICENSE-APACHE | 202 + Makefile | 30 + README.md | 15 + SOURCE | 1 + STM32Loader/checkArtooAndUpdate.py | 292 + STM32Loader/checkArtooAndUpdateTest | 96 + STM32Loader/reset_artoo | 63 + STM32Loader/stm32_check_cal.py | 36 + STM32Loader/stm32_readout_protect.py | 15 + STM32Loader/stm32_reset.py | 14 + STM32Loader/stm32_update_test.sh | 149 + STM32Loader/test_save_stick_cal.sh | 24 + STM32Loader/updateArtoo.sh | 28 + config/checknet | 34 + config/configinit | 285 + config/logrotate-sololink.conf.controller | 23 + config/logrotate-sololink.conf.solo | 25 + config/max_dgram_qlen | 3 + config/sololink | 5 + config/sololink.orig | 191 + config/sololink_config | 1289 ++++ config/sololink_config_sample | 28 + config/sololink_config_test | 689 ++ config/syslog.conf.busybox.controller | 9 + config/syslog.conf.busybox.solo | 11 + config/test_3dr_README | 2 + config/test_3dr_controller/STM_VERSION | 1 + config/test_3dr_controller/VERSION | 1 + config/test_3dr_controller/etc/hostapd.conf | 1749 +++++ .../test_3dr_controller/etc/hostapd.conf.md5 | 1 + .../mnt/rootfs.ro/etc/hostapd.orig | 1749 +++++ config/test_3dr_solo/PIX_VERSION | 1 + config/test_3dr_solo/VERSION | 1 + config/test_3dr_solo/etc/wpa_supplicant.conf | 6 + .../test_3dr_solo/etc/wpa_supplicant.conf.md5 | 1 + .../mnt/rootfs.ro/etc/wpa_supplicant.orig | 6 + config/test_configinit | 244 + flightcode/Makefile | 51 + flightcode/arp_table/Makefile | 40 + flightcode/arp_table/main.c | 33 + flightcode/baudcheck/Makefile | 37 + flightcode/baudcheck/baudcheck.cpp | 371 ++ flightcode/command/Commander.cpp | 370 ++ flightcode/command/Commander.h | 128 + flightcode/command/Makefile | 38 + flightcode/dataflash_logger/Makefile | 56 + flightcode/dataflash_logger/MsgHandler.cpp | 264 + flightcode/dataflash_logger/MsgHandler.h | 152 + flightcode/dataflash_logger/analyzer_util.cpp | 76 + flightcode/dataflash_logger/analyzer_util.h | 75 + .../analyzervehicle_copter.cpp | 127 + flightcode/dataflash_logger/common_tool.cpp | 173 + flightcode/dataflash_logger/common_tool.h | 44 + .../dataflash_logger/dataflash_logger.cpp | 477 ++ .../dataflash_logger/dataflash_logger.h | 119 + .../dataflash_logger_program.cpp | 167 + .../dataflash_logger_program.h | 49 + .../dataflash_message_handler.cpp | 1 + .../dataflash_message_handler.h | 31 + flightcode/dataflash_logger/format_reader.cpp | 61 + flightcode/dataflash_logger/format_reader.h | 49 + flightcode/dataflash_logger/heart.cpp | 39 + flightcode/dataflash_logger/heart.h | 34 + flightcode/dataflash_logger/la-log.cpp | 82 + flightcode/dataflash_logger/la-log.h | 45 + .../mavlink_message_handler.cpp | 119 + .../mavlink_message_handler.h | 98 + .../dataflash_logger/mavlink_reader.cpp | 82 + flightcode/dataflash_logger/mavlink_reader.h | 61 + .../dataflash_logger/mavlink_writer.cpp | 29 + flightcode/dataflash_logger/mavlink_writer.h | 27 + .../dataflash_logger/message_handler.cpp | 0 flightcode/dataflash_logger/message_handler.h | 39 + flightcode/dataflash_logger/telem_client.cpp | 13 + flightcode/dataflash_logger/telem_client.h | 44 + .../telem_forwarder_client.cpp | 174 + .../dataflash_logger/telem_forwarder_client.h | 47 + flightcode/dataflash_logger/telem_serial.cpp | 217 + flightcode/dataflash_logger/telem_serial.h | 44 + flightcode/dflog/Makefile | 45 + flightcode/dflog/dataFlashMAVLink-to-artoo.py | 173 + flightcode/dflog/dflog_downloader.cpp | 497 ++ flightcode/dflog/loadLog.py | 82 + flightcode/hostapd_ctrl/Makefile | 40 + flightcode/hostapd_ctrl/main.c | 48 + flightcode/ini/Makefile | 14 + flightcode/ini/cpp/INIReader.cpp | 75 + flightcode/ini/cpp/INIReader.h | 51 + flightcode/ini/cpp/Makefile | 14 + flightcode/ini/ini.c | 174 + flightcode/ini/ini.h | 75 + flightcode/log/Log.cpp | 160 + flightcode/log/Log.h | 41 + flightcode/log/Makefile | 14 + .../c_library/ardupilotmega/ardupilotmega.h | 708 ++ .../mavlink/c_library/ardupilotmega/mavlink.h | 27 + .../ardupilotmega/mavlink_msg_ahrs.h | 353 + .../ardupilotmega/mavlink_msg_ahrs2.h | 329 + .../ardupilotmega/mavlink_msg_ahrs3.h | 425 ++ .../mavlink_msg_airspeed_autocal.h | 473 ++ .../ardupilotmega/mavlink_msg_ap_adc.h | 329 + .../mavlink_msg_autopilot_version_request.h | 233 + .../ardupilotmega/mavlink_msg_battery2.h | 233 + .../mavlink_msg_camera_feedback.h | 497 ++ .../ardupilotmega/mavlink_msg_camera_status.h | 401 ++ .../mavlink_msg_compassmot_status.h | 329 + .../ardupilotmega/mavlink_msg_data16.h | 249 + .../ardupilotmega/mavlink_msg_data32.h | 249 + .../ardupilotmega/mavlink_msg_data64.h | 249 + .../ardupilotmega/mavlink_msg_data96.h | 249 + .../mavlink_msg_digicam_configure.h | 449 ++ .../mavlink_msg_digicam_control.h | 425 ++ .../mavlink_msg_ekf_status_report.h | 329 + .../mavlink_msg_fence_fetch_point.h | 257 + .../ardupilotmega/mavlink_msg_fence_point.h | 329 + .../ardupilotmega/mavlink_msg_fence_status.h | 281 + .../mavlink_msg_gimbal_control.h | 305 + .../ardupilotmega/mavlink_msg_gimbal_report.h | 473 ++ .../mavlink_msg_gimbal_torque_cmd_report.h | 305 + .../mavlink_msg_gopro_get_request.h | 257 + .../mavlink_msg_gopro_get_response.h | 249 + .../mavlink_msg_gopro_heartbeat.h | 257 + .../mavlink_msg_gopro_set_request.h | 273 + .../mavlink_msg_gopro_set_response.h | 233 + .../ardupilotmega/mavlink_msg_gps_accuracy.h | 353 + .../ardupilotmega/mavlink_msg_hwstatus.h | 233 + .../ardupilotmega/mavlink_msg_led_control.h | 321 + .../ardupilotmega/mavlink_msg_limits_status.h | 401 ++ .../mavlink_msg_mag_cal_progress.h | 393 ++ .../mavlink_msg_mag_cal_report.h | 521 ++ .../ardupilotmega/mavlink_msg_meminfo.h | 233 + .../mavlink_msg_mount_configure.h | 329 + .../ardupilotmega/mavlink_msg_mount_control.h | 329 + .../ardupilotmega/mavlink_msg_mount_status.h | 305 + .../ardupilotmega/mavlink_msg_pid_tuning.h | 353 + .../ardupilotmega/mavlink_msg_radio.h | 353 + .../mavlink_msg_rally_fetch_point.h | 257 + .../ardupilotmega/mavlink_msg_rally_point.h | 425 ++ .../ardupilotmega/mavlink_msg_rangefinder.h | 233 + .../mavlink_msg_remote_log_block_status.h | 281 + .../mavlink_msg_remote_log_data_block.h | 273 + .../mavlink_msg_sensor_offsets.h | 473 ++ .../mavlink_msg_set_mag_offsets.h | 305 + .../ardupilotmega/mavlink_msg_simstate.h | 449 ++ .../ardupilotmega/mavlink_msg_wind.h | 257 + .../c_library/ardupilotmega/testsuite.h | 2437 +++++++ .../mavlink/c_library/ardupilotmega/version.h | 12 + flightcode/mavlink/c_library/checksum.h | 96 + flightcode/mavlink/c_library/common/common.h | 757 +++ flightcode/mavlink/c_library/common/mavlink.h | 27 + .../mavlink_msg_actuator_control_target.h | 249 + .../common/mavlink_msg_att_pos_mocap.h | 297 + .../c_library/common/mavlink_msg_attitude.h | 353 + .../common/mavlink_msg_attitude_quaternion.h | 377 ++ .../mavlink_msg_attitude_quaternion_cov.h | 322 + .../common/mavlink_msg_attitude_target.h | 345 + .../c_library/common/mavlink_msg_auth_key.h | 209 + .../common/mavlink_msg_autopilot_version.h | 443 ++ .../common/mavlink_msg_battery_status.h | 393 ++ .../common/mavlink_msg_camera_trigger.h | 233 + .../mavlink_msg_change_operator_control.h | 273 + .../mavlink_msg_change_operator_control_ack.h | 257 + .../common/mavlink_msg_command_ack.h | 233 + .../common/mavlink_msg_command_int.h | 497 ++ .../common/mavlink_msg_command_long.h | 449 ++ .../common/mavlink_msg_data_stream.h | 257 + .../mavlink_msg_data_transmission_handshake.h | 353 + .../c_library/common/mavlink_msg_debug.h | 257 + .../c_library/common/mavlink_msg_debug_vect.h | 297 + .../common/mavlink_msg_distance_sensor.h | 377 ++ .../common/mavlink_msg_encapsulated_data.h | 225 + .../mavlink_msg_file_transfer_protocol.h | 273 + .../common/mavlink_msg_global_position_int.h | 401 ++ .../mavlink_msg_global_position_int_cov.h | 441 ++ ...link_msg_global_vision_position_estimate.h | 353 + .../c_library/common/mavlink_msg_gps2_raw.h | 473 ++ .../c_library/common/mavlink_msg_gps2_rtk.h | 497 ++ .../common/mavlink_msg_gps_global_origin.h | 257 + .../common/mavlink_msg_gps_inject_data.h | 273 + .../common/mavlink_msg_gps_raw_int.h | 425 ++ .../c_library/common/mavlink_msg_gps_rtk.h | 497 ++ .../c_library/common/mavlink_msg_gps_status.h | 325 + .../c_library/common/mavlink_msg_heartbeat.h | 326 + .../common/mavlink_msg_highres_imu.h | 545 ++ .../common/mavlink_msg_hil_controls.h | 449 ++ .../c_library/common/mavlink_msg_hil_gps.h | 497 ++ .../common/mavlink_msg_hil_optical_flow.h | 473 ++ .../common/mavlink_msg_hil_rc_inputs_raw.h | 521 ++ .../c_library/common/mavlink_msg_hil_sensor.h | 545 ++ .../c_library/common/mavlink_msg_hil_state.h | 569 ++ .../common/mavlink_msg_hil_state_quaternion.h | 561 ++ .../common/mavlink_msg_landing_target.h | 305 + .../common/mavlink_msg_local_position_ned.h | 353 + .../mavlink_msg_local_position_ned_cov.h | 489 ++ ..._local_position_ned_system_global_offset.h | 353 + .../c_library/common/mavlink_msg_log_data.h | 273 + .../c_library/common/mavlink_msg_log_entry.h | 305 + .../c_library/common/mavlink_msg_log_erase.h | 233 + .../common/mavlink_msg_log_request_data.h | 305 + .../common/mavlink_msg_log_request_end.h | 233 + .../common/mavlink_msg_log_request_list.h | 281 + .../common/mavlink_msg_manual_control.h | 329 + .../common/mavlink_msg_manual_setpoint.h | 353 + .../common/mavlink_msg_memory_vect.h | 273 + .../common/mavlink_msg_mission_ack.h | 257 + .../common/mavlink_msg_mission_clear_all.h | 233 + .../common/mavlink_msg_mission_count.h | 257 + .../common/mavlink_msg_mission_current.h | 209 + .../common/mavlink_msg_mission_item.h | 521 ++ .../common/mavlink_msg_mission_item_int.h | 521 ++ .../common/mavlink_msg_mission_item_reached.h | 209 + .../common/mavlink_msg_mission_request.h | 257 + .../common/mavlink_msg_mission_request_list.h | 233 + ...mavlink_msg_mission_request_partial_list.h | 281 + .../common/mavlink_msg_mission_set_current.h | 257 + .../mavlink_msg_mission_write_partial_list.h | 281 + .../common/mavlink_msg_named_value_float.h | 249 + .../common/mavlink_msg_named_value_int.h | 249 + .../mavlink_msg_nav_controller_output.h | 377 ++ .../common/mavlink_msg_optical_flow.h | 377 ++ .../common/mavlink_msg_optical_flow_rad.h | 473 ++ .../common/mavlink_msg_param_map_rc.h | 393 ++ .../common/mavlink_msg_param_request_list.h | 233 + .../common/mavlink_msg_param_request_read.h | 273 + .../c_library/common/mavlink_msg_param_set.h | 297 + .../common/mavlink_msg_param_value.h | 297 + .../c_library/common/mavlink_msg_ping.h | 281 + .../mavlink_msg_position_target_global_int.h | 521 ++ .../mavlink_msg_position_target_local_ned.h | 521 ++ .../common/mavlink_msg_power_status.h | 257 + .../common/mavlink_msg_radio_status.h | 353 + .../c_library/common/mavlink_msg_raw_imu.h | 425 ++ .../common/mavlink_msg_raw_pressure.h | 305 + .../common/mavlink_msg_rc_channels.h | 689 ++ .../common/mavlink_msg_rc_channels_override.h | 425 ++ .../common/mavlink_msg_rc_channels_raw.h | 449 ++ .../common/mavlink_msg_rc_channels_scaled.h | 449 ++ .../common/mavlink_msg_request_data_stream.h | 305 + .../common/mavlink_msg_safety_allowed_area.h | 353 + .../mavlink_msg_safety_set_allowed_area.h | 401 ++ .../c_library/common/mavlink_msg_scaled_imu.h | 425 ++ .../common/mavlink_msg_scaled_imu2.h | 425 ++ .../common/mavlink_msg_scaled_imu3.h | 425 ++ .../common/mavlink_msg_scaled_pressure.h | 281 + .../common/mavlink_msg_scaled_pressure2.h | 281 + .../common/mavlink_msg_serial_control.h | 321 + .../common/mavlink_msg_servo_output_raw.h | 425 ++ .../mavlink_msg_set_actuator_control_target.h | 297 + .../common/mavlink_msg_set_attitude_target.h | 393 ++ .../mavlink_msg_set_gps_global_origin.h | 281 + .../c_library/common/mavlink_msg_set_mode.h | 257 + ...vlink_msg_set_position_target_global_int.h | 569 ++ ...avlink_msg_set_position_target_local_ned.h | 569 ++ .../c_library/common/mavlink_msg_sim_state.h | 689 ++ .../c_library/common/mavlink_msg_statustext.h | 225 + .../c_library/common/mavlink_msg_sys_status.h | 497 ++ .../common/mavlink_msg_system_time.h | 233 + .../common/mavlink_msg_terrain_check.h | 233 + .../common/mavlink_msg_terrain_data.h | 297 + .../common/mavlink_msg_terrain_report.h | 353 + .../common/mavlink_msg_terrain_request.h | 281 + .../c_library/common/mavlink_msg_timesync.h | 233 + .../common/mavlink_msg_v2_extension.h | 297 + .../c_library/common/mavlink_msg_vfr_hud.h | 329 + .../mavlink_msg_vicon_position_estimate.h | 353 + .../mavlink_msg_vision_position_estimate.h | 353 + .../mavlink_msg_vision_speed_estimate.h | 281 + .../mavlink/c_library/common/testsuite.h | 5904 +++++++++++++++++ flightcode/mavlink/c_library/common/version.h | 12 + .../mavlink/c_library/mavlink_conversions.h | 211 + .../mavlink/c_library/mavlink_helpers.h | 676 ++ flightcode/mavlink/c_library/mavlink_types.h | 228 + flightcode/mavlink/c_library/protocol.h | 339 + flightcode/mavlink/generate | 31 + flightcode/mavpkt/Makefile | 34 + flightcode/mavpkt/mavpkt.cpp | 712 ++ flightcode/old/app_streamer.py | 103 + flightcode/old/logd.py | 436 ++ flightcode/old/mavproxy_solo.py | 137 + flightcode/old/pixrc.py | 378 ++ flightcode/old/rssi_send.py | 129 + flightcode/old/runMavproxy.sh | 20 + flightcode/old/simple_stats.py | 49 + flightcode/old/slip.py | 192 + flightcode/old/stm32.py | 812 +++ flightcode/old/stm32_sim.py | 161 + flightcode/pixrc/Makefile | 53 + flightcode/pixrc/RcCommander.cpp | 42 + flightcode/pixrc/RcCommander.h | 23 + flightcode/pixrc/pixrc.cpp | 1158 ++++ flightcode/pixrc/rc_ipc.c | 196 + flightcode/pixrc/rc_ipc.h | 28 + flightcode/pixrc/rc_pkt.h | 30 + flightcode/pixrc/test_pixrc.cpp | 87 + flightcode/proc_top/Makefile | 40 + flightcode/proc_top/main.c | 117 + flightcode/python/app_server.py | 186 + flightcode/python/ath9k.py | 73 + flightcode/python/attic/checkPixBaud.py | 136 + flightcode/python/gpio.py | 48 + flightcode/python/led_nop.py | 6 + flightcode/python/led_solo.py | 9 + flightcode/python/lsproc.py | 329 + flightcode/python/pixhawk.py | 733 ++ flightcode/python/rc_lock.py | 38 + flightcode/python/runStickCal.sh | 18 + flightcode/python/slip.py | 72 + flightcode/python/stick-cal.py | 92 + flightcode/python/stm32_defs.py | 12 + flightcode/python/usb_nop.py | 13 + flightcode/python/usb_solo.py | 26 + flightcode/rssi/Makefile | 40 + flightcode/rssi/rssi_send.cpp | 859 +++ flightcode/stm32/.gitignore | 1 + flightcode/stm32/AppConnected.h | 23 + flightcode/stm32/ButtonEventHandler.cpp | 31 + flightcode/stm32/ButtonEventHandler.h | 28 + flightcode/stm32/ButtonFunctionCfg.cpp | 27 + flightcode/stm32/ButtonFunctionCfg.h | 42 + flightcode/stm32/CircularBuffer.cpp | 10 + flightcode/stm32/CircularBuffer.h | 131 + flightcode/stm32/CircularBufferTest.cpp | 167 + flightcode/stm32/ConfigStickAxes.cpp | 11 + flightcode/stm32/ConfigStickAxes.h | 14 + flightcode/stm32/ConfigSweepTime.cpp | 11 + flightcode/stm32/ConfigSweepTime.h | 14 + flightcode/stm32/InputReport.cpp | 54 + flightcode/stm32/InputReport.h | 64 + flightcode/stm32/LockoutState.cpp | 20 + flightcode/stm32/LockoutState.h | 29 + flightcode/stm32/Makefile | 75 + flightcode/stm32/PacketHandler.cpp | 119 + flightcode/stm32/PacketHandler.h | 44 + flightcode/stm32/PairReq.cpp | 29 + flightcode/stm32/PairReq.h | 26 + flightcode/stm32/PairRes.h | 23 + flightcode/stm32/ParamStoredVals.h | 13 + flightcode/stm32/RC.cpp | 166 + flightcode/stm32/RC.h | 42 + flightcode/stm32/SLIP.cpp | 124 + flightcode/stm32/SLIP.h | 42 + flightcode/stm32/SerialLog.cpp | 198 + flightcode/stm32/SerialLog.h | 114 + flightcode/stm32/SerialLogTest.cpp | 45 + flightcode/stm32/SetShotInfo.cpp | 22 + flightcode/stm32/SetShotInfo.h | 36 + flightcode/stm32/SetTelemUnits.cpp | 21 + flightcode/stm32/SetTelemUnits.h | 16 + flightcode/stm32/SoloMessage.cpp | 51 + flightcode/stm32/SoloMessage.h | 54 + flightcode/stm32/SysInfo.cpp | 76 + flightcode/stm32/SysInfo.h | 30 + flightcode/stm32/TcpServer.cpp | 642 ++ flightcode/stm32/TcpServer.h | 108 + flightcode/stm32/Telem.cpp | 146 + flightcode/stm32/Telem.h | 36 + flightcode/stm32/Updater.h | 26 + flightcode/stm32/app_connected_msg.py | 43 + flightcode/stm32/btn_client.py | 57 + flightcode/stm32/config_stick_axes.py | 120 + flightcode/stm32/config_stick_axes_msg.py | 28 + flightcode/stm32/config_sweep_time.py | 63 + flightcode/stm32/config_sweep_time_msg.py | 23 + flightcode/stm32/input_report_client.py | 44 + flightcode/stm32/input_report_msg.py | 82 + flightcode/stm32/lockout_msg.py | 43 + flightcode/stm32/packetTypes.h | 38 + flightcode/stm32/param_stored_vals_msg.py | 147 + flightcode/stm32/set_telem_units.py | 29 + flightcode/stm32/set_telem_units_msg.py | 25 + flightcode/stm32/stm32.cpp | 790 +++ flightcode/stm32/updater_msg.py | 49 + flightcode/telem/Makefile | 48 + flightcode/telem/telem_forward.cpp | 1458 ++++ flightcode/telem/test_telem.cpp | 73 + flightcode/telem_ctrl/Makefile | 48 + flightcode/telem_ctrl/telem_ctrl.cpp | 933 +++ flightcode/telem_ctrl/telem_dest.cpp | 152 + flightcode/telem_ctrl/telem_dest.h | 55 + flightcode/telem_test/Makefile | 36 + flightcode/telem_test/telem_test.cpp | 351 + flightcode/thermal/log_temp | 8 + flightcode/tlog/Makefile | 45 + flightcode/tlog/tlog.cpp | 312 + flightcode/unlock/Makefile | 37 + flightcode/unlock/unlock.cpp | 104 + flightcode/util/ButtonEventMessage.cpp | 15 + flightcode/util/ButtonEventMessage.h | 75 + flightcode/util/Makefile | 35 + flightcode/util/RcLock.cpp | 48 + flightcode/util/RcLock.h | 14 + flightcode/util/arp_table.c | 171 + flightcode/util/arp_table.h | 32 + flightcode/util/file_util.c | 38 + flightcode/util/file_util.h | 15 + flightcode/util/hostapd_ctrl.c | 192 + flightcode/util/hostapd_ctrl.h | 29 + flightcode/util/link_packet.h | 24 + flightcode/util/main.c | 23 + flightcode/util/mutex.c | 194 + flightcode/util/mutex.h | 20 + flightcode/util/net_stats.c | 141 + flightcode/util/net_stats.h | 18 + flightcode/util/net_wmm.h | 18 + flightcode/util/proc_table.c | 317 + flightcode/util/proc_table.h | 78 + flightcode/util/syslog.c | 274 + flightcode/util/syslog_test.c | 164 + flightcode/util/syslog_test.h | 14 + flightcode/util/util.c | 131 + flightcode/util/util.h | 31 + flightcode/util/util_test.c | 35 + flightcode/util/util_test.h | 14 + flightcode/video/README | 54 + flightcode/video/app/Makefile | 45 + flightcode/video/app/app_streamer.cpp | 334 + flightcode/video/cleanLibs.sh | 24 + flightcode/video/hdmi/Makefile | 33 + flightcode/video/hdmi/hdmiout.c | 318 + flightcode/video/vid/80211.cpp | 382 ++ flightcode/video/vid/80211.h | 6 + flightcode/video/vid/Makefile | 48 + flightcode/video/vid/vidlaunch.cpp | 587 ++ flightcode/wdog/Makefile | 36 + flightcode/wdog/wdog.c | 106 + gimbal/firmware_helper.py | 60 + gimbal/firmware_loader.py | 140 + gimbal/loadGimbal.py | 165 + gimbal/setup.py | 478 ++ gimbal/setup_comutation.py | 97 + gimbal/setup_factory_pub.py | 36 + gimbal/setup_mavlink.py | 220 + gimbal/setup_param.py | 189 + gimbal/setup_validate.py | 278 + init/3dr_rotate | 41 + init/clock_sync | 27 + init/pixhawk | 3 + init/shutdownArtoo.sh | 29 + init/startwd | 3 + init/updateGimbal.sh | 2 + net/etc/init.d/hostapd | 84 + net/etc/init.d/netinit | 25 + net/etc/init.d/networking | 117 + net/usr/bin/.gitignore | 1 + net/usr/bin/clock.py | 75 + net/usr/bin/configfile.py | 110 + net/usr/bin/getmaclocal.py | 22 + net/usr/bin/hostapdconfig.py | 103 + net/usr/bin/ip.py | 77 + net/usr/bin/iw.py | 119 + net/usr/bin/rc_cli.py | 29 + net/usr/bin/rc_remap_sample.py | 83 + net/usr/bin/telem_ctrl.py | 406 ++ net/usr/bin/wpa_supplicant.py | 292 + pair/hostapd_ctrl.py | 113 + pair/ifconfig.py | 36 + pair/ip_util.py | 39 + pair/pair.py | 51 + pair/pair_button.py | 55 + pair/pair_confirm.py | 6 + pair/pair_server.py | 667 ++ pair/pair_solo.py | 578 ++ pair/runlevel.py | 14 + pair/udhcpc.py | 79 + pair/wpa_cli.py | 139 + pair/wpa_control.py | 158 + pair/wpa_supplicant_init | 19 + pair/wps_confirm.py | 28 + pair/wps_pair_msg.py | 54 + px_uploader/loadPixhawk.py | 181 + px_uploader/px_uploader.py | 613 ++ tools/3dcp | 22 + tools/3dsh | 18 + tools/build/clang-format-run.py | 52 + tools/build/sololink_new | 82 + tools/logdownload/grabSoloLinkLogs.sh | 14 + tools/scripts/px_usb_switch.py | 54 + tools/setregdomain/setCountryCode.sh | 90 + tools/updater/.gitignore | 1 + tools/updater/fetchLatest.sh | 46 + tools/updater/makeGolden.sh | 31 + tools/updater/unpair | 27 + tools/updater/updateFuncs | 198 + tools/updater/updateSoloAndArtoo.sh | 651 ++ tools/updater/updateTest | 101 + tools/wifiTest/ArtooTest.sh | 140 + tools/wifiTest/SoloTest.sh | 124 + wifi/Makefile | 32 + wifi/logRCUp.sh | 34 + wifi/survey_dump.c | 313 + wifi/survey_log | 16 + wifi/wifistats.c | 119 + 495 files changed, 110498 insertions(+) create mode 100644 .clang-format create mode 100644 .gitignore create mode 100644 COPYRIGHT-3DR create mode 100644 LICENSE-APACHE create mode 100644 Makefile create mode 100644 README.md create mode 100644 SOURCE create mode 100755 STM32Loader/checkArtooAndUpdate.py create mode 100755 STM32Loader/checkArtooAndUpdateTest create mode 100755 STM32Loader/reset_artoo create mode 100755 STM32Loader/stm32_check_cal.py create mode 100755 STM32Loader/stm32_readout_protect.py create mode 100755 STM32Loader/stm32_reset.py create mode 100755 STM32Loader/stm32_update_test.sh create mode 100755 STM32Loader/test_save_stick_cal.sh create mode 100755 STM32Loader/updateArtoo.sh create mode 100755 config/checknet create mode 100755 config/configinit create mode 100644 config/logrotate-sololink.conf.controller create mode 100644 config/logrotate-sololink.conf.solo create mode 100755 config/max_dgram_qlen create mode 100644 config/sololink create mode 100644 config/sololink.orig create mode 100755 config/sololink_config create mode 100755 config/sololink_config_sample create mode 100755 config/sololink_config_test create mode 100644 config/syslog.conf.busybox.controller create mode 100644 config/syslog.conf.busybox.solo create mode 100644 config/test_3dr_README create mode 100644 config/test_3dr_controller/STM_VERSION create mode 100644 config/test_3dr_controller/VERSION create mode 100644 config/test_3dr_controller/etc/hostapd.conf create mode 100644 config/test_3dr_controller/etc/hostapd.conf.md5 create mode 100644 config/test_3dr_controller/mnt/rootfs.ro/etc/hostapd.orig create mode 100644 config/test_3dr_solo/PIX_VERSION create mode 100644 config/test_3dr_solo/VERSION create mode 100644 config/test_3dr_solo/etc/wpa_supplicant.conf create mode 100644 config/test_3dr_solo/etc/wpa_supplicant.conf.md5 create mode 100644 config/test_3dr_solo/mnt/rootfs.ro/etc/wpa_supplicant.orig create mode 100755 config/test_configinit create mode 100644 flightcode/Makefile create mode 100644 flightcode/arp_table/Makefile create mode 100644 flightcode/arp_table/main.c create mode 100644 flightcode/baudcheck/Makefile create mode 100644 flightcode/baudcheck/baudcheck.cpp create mode 100644 flightcode/command/Commander.cpp create mode 100644 flightcode/command/Commander.h create mode 100644 flightcode/command/Makefile create mode 100644 flightcode/dataflash_logger/Makefile create mode 100644 flightcode/dataflash_logger/MsgHandler.cpp create mode 100644 flightcode/dataflash_logger/MsgHandler.h create mode 100644 flightcode/dataflash_logger/analyzer_util.cpp create mode 100644 flightcode/dataflash_logger/analyzer_util.h create mode 100644 flightcode/dataflash_logger/analyzervehicle_copter.cpp create mode 100644 flightcode/dataflash_logger/common_tool.cpp create mode 100644 flightcode/dataflash_logger/common_tool.h create mode 100644 flightcode/dataflash_logger/dataflash_logger.cpp create mode 100644 flightcode/dataflash_logger/dataflash_logger.h create mode 100644 flightcode/dataflash_logger/dataflash_logger_program.cpp create mode 100644 flightcode/dataflash_logger/dataflash_logger_program.h create mode 100644 flightcode/dataflash_logger/dataflash_message_handler.cpp create mode 100644 flightcode/dataflash_logger/dataflash_message_handler.h create mode 100644 flightcode/dataflash_logger/format_reader.cpp create mode 100644 flightcode/dataflash_logger/format_reader.h create mode 100644 flightcode/dataflash_logger/heart.cpp create mode 100644 flightcode/dataflash_logger/heart.h create mode 100644 flightcode/dataflash_logger/la-log.cpp create mode 100644 flightcode/dataflash_logger/la-log.h create mode 100644 flightcode/dataflash_logger/mavlink_message_handler.cpp create mode 100644 flightcode/dataflash_logger/mavlink_message_handler.h create mode 100644 flightcode/dataflash_logger/mavlink_reader.cpp create mode 100644 flightcode/dataflash_logger/mavlink_reader.h create mode 100644 flightcode/dataflash_logger/mavlink_writer.cpp create mode 100644 flightcode/dataflash_logger/mavlink_writer.h create mode 100644 flightcode/dataflash_logger/message_handler.cpp create mode 100644 flightcode/dataflash_logger/message_handler.h create mode 100644 flightcode/dataflash_logger/telem_client.cpp create mode 100644 flightcode/dataflash_logger/telem_client.h create mode 100644 flightcode/dataflash_logger/telem_forwarder_client.cpp create mode 100644 flightcode/dataflash_logger/telem_forwarder_client.h create mode 100644 flightcode/dataflash_logger/telem_serial.cpp create mode 100644 flightcode/dataflash_logger/telem_serial.h create mode 100644 flightcode/dflog/Makefile create mode 100755 flightcode/dflog/dataFlashMAVLink-to-artoo.py create mode 100644 flightcode/dflog/dflog_downloader.cpp create mode 100755 flightcode/dflog/loadLog.py create mode 100644 flightcode/hostapd_ctrl/Makefile create mode 100644 flightcode/hostapd_ctrl/main.c create mode 100644 flightcode/ini/Makefile create mode 100644 flightcode/ini/cpp/INIReader.cpp create mode 100644 flightcode/ini/cpp/INIReader.h create mode 100644 flightcode/ini/cpp/Makefile create mode 100644 flightcode/ini/ini.c create mode 100644 flightcode/ini/ini.h create mode 100644 flightcode/log/Log.cpp create mode 100644 flightcode/log/Log.h create mode 100644 flightcode/log/Makefile create mode 100644 flightcode/mavlink/c_library/ardupilotmega/ardupilotmega.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs2.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs3.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_airspeed_autocal.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ap_adc.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_autopilot_version_request.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_battery2.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_feedback.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_compassmot_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data16.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data32.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data64.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data96.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_configure.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_control.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ekf_status_report.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_fetch_point.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_point.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_control.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_report.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_torque_cmd_report.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_request.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_response.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_heartbeat.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_request.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_response.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gps_accuracy.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_hwstatus.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_led_control.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_limits_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_progress.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_report.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_meminfo.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_configure.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_control.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_pid_tuning.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_radio.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_fetch_point.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_point.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rangefinder.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_block_status.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_data_block.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_sensor_offsets.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_set_mag_offsets.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_simstate.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_wind.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/testsuite.h create mode 100644 flightcode/mavlink/c_library/ardupilotmega/version.h create mode 100644 flightcode/mavlink/c_library/checksum.h create mode 100644 flightcode/mavlink/c_library/common/common.h create mode 100644 flightcode/mavlink/c_library/common/mavlink.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_actuator_control_target.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_att_pos_mocap.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_attitude.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion_cov.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_attitude_target.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_auth_key.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_autopilot_version.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_battery_status.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_camera_trigger.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control_ack.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_command_ack.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_command_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_command_long.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_data_stream.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_data_transmission_handshake.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_debug.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_debug_vect.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_distance_sensor.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_encapsulated_data.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_file_transfer_protocol.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_global_position_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_global_position_int_cov.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_global_vision_position_estimate.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps2_raw.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps2_rtk.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps_global_origin.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps_inject_data.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps_raw_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps_rtk.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_gps_status.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_heartbeat.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_highres_imu.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_controls.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_gps.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_optical_flow.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_rc_inputs_raw.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_sensor.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_state.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_hil_state_quaternion.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_landing_target.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_cov.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_system_global_offset.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_data.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_entry.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_erase.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_request_data.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_request_end.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_log_request_list.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_manual_control.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_manual_setpoint.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_memory_vect.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_ack.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_clear_all.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_count.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_current.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_item.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_item_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_item_reached.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_request.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_request_list.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_request_partial_list.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_set_current.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_mission_write_partial_list.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_named_value_float.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_named_value_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_nav_controller_output.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_optical_flow.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_optical_flow_rad.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_param_map_rc.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_param_request_list.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_param_request_read.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_param_set.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_param_value.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_ping.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_position_target_global_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_position_target_local_ned.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_power_status.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_radio_status.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_raw_imu.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_raw_pressure.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_rc_channels.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_override.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_raw.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_scaled.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_request_data_stream.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_safety_allowed_area.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_safety_set_allowed_area.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu2.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu3.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure2.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_serial_control.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_servo_output_raw.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_actuator_control_target.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_attitude_target.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_gps_global_origin.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_mode.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_global_int.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_local_ned.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_sim_state.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_statustext.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_sys_status.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_system_time.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_terrain_check.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_terrain_data.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_terrain_report.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_terrain_request.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_timesync.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_v2_extension.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_vfr_hud.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_vicon_position_estimate.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_vision_position_estimate.h create mode 100644 flightcode/mavlink/c_library/common/mavlink_msg_vision_speed_estimate.h create mode 100644 flightcode/mavlink/c_library/common/testsuite.h create mode 100644 flightcode/mavlink/c_library/common/version.h create mode 100644 flightcode/mavlink/c_library/mavlink_conversions.h create mode 100644 flightcode/mavlink/c_library/mavlink_helpers.h create mode 100644 flightcode/mavlink/c_library/mavlink_types.h create mode 100644 flightcode/mavlink/c_library/protocol.h create mode 100755 flightcode/mavlink/generate create mode 100644 flightcode/mavpkt/Makefile create mode 100644 flightcode/mavpkt/mavpkt.cpp create mode 100755 flightcode/old/app_streamer.py create mode 100755 flightcode/old/logd.py create mode 100644 flightcode/old/mavproxy_solo.py create mode 100755 flightcode/old/pixrc.py create mode 100755 flightcode/old/rssi_send.py create mode 100755 flightcode/old/runMavproxy.sh create mode 100755 flightcode/old/simple_stats.py create mode 100755 flightcode/old/slip.py create mode 100755 flightcode/old/stm32.py create mode 100755 flightcode/old/stm32_sim.py create mode 100644 flightcode/pixrc/Makefile create mode 100644 flightcode/pixrc/RcCommander.cpp create mode 100644 flightcode/pixrc/RcCommander.h create mode 100644 flightcode/pixrc/pixrc.cpp create mode 100644 flightcode/pixrc/rc_ipc.c create mode 100644 flightcode/pixrc/rc_ipc.h create mode 100644 flightcode/pixrc/rc_pkt.h create mode 100644 flightcode/pixrc/test_pixrc.cpp create mode 100644 flightcode/proc_top/Makefile create mode 100644 flightcode/proc_top/main.c create mode 100755 flightcode/python/app_server.py create mode 100755 flightcode/python/ath9k.py create mode 100755 flightcode/python/attic/checkPixBaud.py create mode 100644 flightcode/python/gpio.py create mode 100644 flightcode/python/led_nop.py create mode 100644 flightcode/python/led_solo.py create mode 100755 flightcode/python/lsproc.py create mode 100755 flightcode/python/pixhawk.py create mode 100644 flightcode/python/rc_lock.py create mode 100755 flightcode/python/runStickCal.sh create mode 100755 flightcode/python/slip.py create mode 100755 flightcode/python/stick-cal.py create mode 100644 flightcode/python/stm32_defs.py create mode 100644 flightcode/python/usb_nop.py create mode 100644 flightcode/python/usb_solo.py create mode 100644 flightcode/rssi/Makefile create mode 100644 flightcode/rssi/rssi_send.cpp create mode 100644 flightcode/stm32/.gitignore create mode 100644 flightcode/stm32/AppConnected.h create mode 100644 flightcode/stm32/ButtonEventHandler.cpp create mode 100644 flightcode/stm32/ButtonEventHandler.h create mode 100644 flightcode/stm32/ButtonFunctionCfg.cpp create mode 100644 flightcode/stm32/ButtonFunctionCfg.h create mode 100644 flightcode/stm32/CircularBuffer.cpp create mode 100644 flightcode/stm32/CircularBuffer.h create mode 100644 flightcode/stm32/CircularBufferTest.cpp create mode 100644 flightcode/stm32/ConfigStickAxes.cpp create mode 100644 flightcode/stm32/ConfigStickAxes.h create mode 100644 flightcode/stm32/ConfigSweepTime.cpp create mode 100644 flightcode/stm32/ConfigSweepTime.h create mode 100644 flightcode/stm32/InputReport.cpp create mode 100644 flightcode/stm32/InputReport.h create mode 100644 flightcode/stm32/LockoutState.cpp create mode 100644 flightcode/stm32/LockoutState.h create mode 100644 flightcode/stm32/Makefile create mode 100644 flightcode/stm32/PacketHandler.cpp create mode 100644 flightcode/stm32/PacketHandler.h create mode 100644 flightcode/stm32/PairReq.cpp create mode 100644 flightcode/stm32/PairReq.h create mode 100644 flightcode/stm32/PairRes.h create mode 100644 flightcode/stm32/ParamStoredVals.h create mode 100644 flightcode/stm32/RC.cpp create mode 100644 flightcode/stm32/RC.h create mode 100644 flightcode/stm32/SLIP.cpp create mode 100644 flightcode/stm32/SLIP.h create mode 100644 flightcode/stm32/SerialLog.cpp create mode 100644 flightcode/stm32/SerialLog.h create mode 100644 flightcode/stm32/SerialLogTest.cpp create mode 100644 flightcode/stm32/SetShotInfo.cpp create mode 100644 flightcode/stm32/SetShotInfo.h create mode 100644 flightcode/stm32/SetTelemUnits.cpp create mode 100644 flightcode/stm32/SetTelemUnits.h create mode 100644 flightcode/stm32/SoloMessage.cpp create mode 100644 flightcode/stm32/SoloMessage.h create mode 100644 flightcode/stm32/SysInfo.cpp create mode 100644 flightcode/stm32/SysInfo.h create mode 100644 flightcode/stm32/TcpServer.cpp create mode 100644 flightcode/stm32/TcpServer.h create mode 100644 flightcode/stm32/Telem.cpp create mode 100644 flightcode/stm32/Telem.h create mode 100644 flightcode/stm32/Updater.h create mode 100755 flightcode/stm32/app_connected_msg.py create mode 100755 flightcode/stm32/btn_client.py create mode 100755 flightcode/stm32/config_stick_axes.py create mode 100644 flightcode/stm32/config_stick_axes_msg.py create mode 100755 flightcode/stm32/config_sweep_time.py create mode 100644 flightcode/stm32/config_sweep_time_msg.py create mode 100755 flightcode/stm32/input_report_client.py create mode 100644 flightcode/stm32/input_report_msg.py create mode 100755 flightcode/stm32/lockout_msg.py create mode 100644 flightcode/stm32/packetTypes.h create mode 100755 flightcode/stm32/param_stored_vals_msg.py create mode 100755 flightcode/stm32/set_telem_units.py create mode 100644 flightcode/stm32/set_telem_units_msg.py create mode 100644 flightcode/stm32/stm32.cpp create mode 100755 flightcode/stm32/updater_msg.py create mode 100644 flightcode/telem/Makefile create mode 100644 flightcode/telem/telem_forward.cpp create mode 100644 flightcode/telem/test_telem.cpp create mode 100644 flightcode/telem_ctrl/Makefile create mode 100644 flightcode/telem_ctrl/telem_ctrl.cpp create mode 100644 flightcode/telem_ctrl/telem_dest.cpp create mode 100644 flightcode/telem_ctrl/telem_dest.h create mode 100644 flightcode/telem_test/Makefile create mode 100644 flightcode/telem_test/telem_test.cpp create mode 100755 flightcode/thermal/log_temp create mode 100644 flightcode/tlog/Makefile create mode 100644 flightcode/tlog/tlog.cpp create mode 100644 flightcode/unlock/Makefile create mode 100644 flightcode/unlock/unlock.cpp create mode 100644 flightcode/util/ButtonEventMessage.cpp create mode 100644 flightcode/util/ButtonEventMessage.h create mode 100644 flightcode/util/Makefile create mode 100644 flightcode/util/RcLock.cpp create mode 100644 flightcode/util/RcLock.h create mode 100644 flightcode/util/arp_table.c create mode 100644 flightcode/util/arp_table.h create mode 100644 flightcode/util/file_util.c create mode 100644 flightcode/util/file_util.h create mode 100644 flightcode/util/hostapd_ctrl.c create mode 100644 flightcode/util/hostapd_ctrl.h create mode 100644 flightcode/util/link_packet.h create mode 100644 flightcode/util/main.c create mode 100644 flightcode/util/mutex.c create mode 100644 flightcode/util/mutex.h create mode 100644 flightcode/util/net_stats.c create mode 100644 flightcode/util/net_stats.h create mode 100644 flightcode/util/net_wmm.h create mode 100644 flightcode/util/proc_table.c create mode 100644 flightcode/util/proc_table.h create mode 100644 flightcode/util/syslog.c create mode 100644 flightcode/util/syslog_test.c create mode 100644 flightcode/util/syslog_test.h create mode 100644 flightcode/util/util.c create mode 100644 flightcode/util/util.h create mode 100644 flightcode/util/util_test.c create mode 100644 flightcode/util/util_test.h create mode 100644 flightcode/video/README create mode 100644 flightcode/video/app/Makefile create mode 100644 flightcode/video/app/app_streamer.cpp create mode 100755 flightcode/video/cleanLibs.sh create mode 100644 flightcode/video/hdmi/Makefile create mode 100644 flightcode/video/hdmi/hdmiout.c create mode 100644 flightcode/video/vid/80211.cpp create mode 100644 flightcode/video/vid/80211.h create mode 100644 flightcode/video/vid/Makefile create mode 100644 flightcode/video/vid/vidlaunch.cpp create mode 100644 flightcode/wdog/Makefile create mode 100644 flightcode/wdog/wdog.c create mode 100644 gimbal/firmware_helper.py create mode 100644 gimbal/firmware_loader.py create mode 100644 gimbal/loadGimbal.py create mode 100755 gimbal/setup.py create mode 100644 gimbal/setup_comutation.py create mode 100644 gimbal/setup_factory_pub.py create mode 100644 gimbal/setup_mavlink.py create mode 100644 gimbal/setup_param.py create mode 100644 gimbal/setup_validate.py create mode 100755 init/3dr_rotate create mode 100755 init/clock_sync create mode 100755 init/pixhawk create mode 100755 init/shutdownArtoo.sh create mode 100755 init/startwd create mode 100755 init/updateGimbal.sh create mode 100755 net/etc/init.d/hostapd create mode 100755 net/etc/init.d/netinit create mode 100755 net/etc/init.d/networking create mode 100644 net/usr/bin/.gitignore create mode 100755 net/usr/bin/clock.py create mode 100755 net/usr/bin/configfile.py create mode 100755 net/usr/bin/getmaclocal.py create mode 100755 net/usr/bin/hostapdconfig.py create mode 100755 net/usr/bin/ip.py create mode 100644 net/usr/bin/iw.py create mode 100755 net/usr/bin/rc_cli.py create mode 100755 net/usr/bin/rc_remap_sample.py create mode 100755 net/usr/bin/telem_ctrl.py create mode 100755 net/usr/bin/wpa_supplicant.py create mode 100644 pair/hostapd_ctrl.py create mode 100644 pair/ifconfig.py create mode 100644 pair/ip_util.py create mode 100644 pair/pair.py create mode 100755 pair/pair_button.py create mode 100755 pair/pair_confirm.py create mode 100755 pair/pair_server.py create mode 100755 pair/pair_solo.py create mode 100644 pair/runlevel.py create mode 100644 pair/udhcpc.py create mode 100644 pair/wpa_cli.py create mode 100644 pair/wpa_control.py create mode 100755 pair/wpa_supplicant_init create mode 100755 pair/wps_confirm.py create mode 100644 pair/wps_pair_msg.py create mode 100755 px_uploader/loadPixhawk.py create mode 100755 px_uploader/px_uploader.py create mode 100755 tools/3dcp create mode 100755 tools/3dsh create mode 100755 tools/build/clang-format-run.py create mode 100755 tools/build/sololink_new create mode 100755 tools/logdownload/grabSoloLinkLogs.sh create mode 100644 tools/scripts/px_usb_switch.py create mode 100755 tools/setregdomain/setCountryCode.sh create mode 100644 tools/updater/.gitignore create mode 100755 tools/updater/fetchLatest.sh create mode 100755 tools/updater/makeGolden.sh create mode 100755 tools/updater/unpair create mode 100755 tools/updater/updateFuncs create mode 100755 tools/updater/updateSoloAndArtoo.sh create mode 100755 tools/updater/updateTest create mode 100755 tools/wifiTest/ArtooTest.sh create mode 100755 tools/wifiTest/SoloTest.sh create mode 100644 wifi/Makefile create mode 100755 wifi/logRCUp.sh create mode 100644 wifi/survey_dump.c create mode 100755 wifi/survey_log create mode 100644 wifi/wifistats.c diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..0c9783e --- /dev/null +++ b/.clang-format @@ -0,0 +1,19 @@ +--- +BasedOnStyle: LLVM +AccessModifierOffset: -4 +AllowShortFunctionsOnASingleLine: false +BreakBeforeBinaryOperators: true +AlwaysBreakTemplateDeclarations: true +ColumnLimit: 100 +Standard: Cpp11 +IndentWidth: 4 +UseTab: Never +BreakBeforeBraces: Linux +#AlignConsecutiveAssignments: true # only available as of clang-format 3.7 +BreakBeforeBinaryOperators: false +PointerAlignment: Right +# SpacesInAngles needed for nested templates (gcc 4.8.4 errors) +SpacesInAngles: true +--- +# other lang configs here... +... diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1ac658 --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +# osx junk +*.DS_Store + +# object files +*.o +*.pyc + +# pycharm files +.idea + +# logs from sim +*.tlog +*.tlog.raw +*.parm diff --git a/COPYRIGHT-3DR b/COPYRIGHT-3DR new file mode 100644 index 0000000..67c241e --- /dev/null +++ b/COPYRIGHT-3DR @@ -0,0 +1,37 @@ +Copyright 2014-2017 3D Robotics + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use these files except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +~~~~ + +Contributors prior to public release: + + * Eric Liao + * John Finley + * Allan Matthew + * Will Silva + * Liam Staskawicz + * Angus Peart + * Igor Napolskikh + * Tim Ryan + * Peter Barker + * Robert Cottrell + * Ramón Roche + * Adam Setapen + * Kevin Mehall + * Jonathan Challinger + * Nick Speal + * Siddharth Bharat Purohit + * Andrew Tridgell + * Daniel Nugent + * Chavi Weingarten diff --git a/LICENSE-APACHE b/LICENSE-APACHE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE-APACHE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..2d7bf71 --- /dev/null +++ b/Makefile @@ -0,0 +1,30 @@ + +SUBDIRS = flightcode wifi +SUBDIRS_BUILD = $(SUBDIRS:%=%_build) +SUBDIRS_CLEAN = $(SUBDIRS:%=%_clean) + +# SUBDIRS2 should eventually be same as SUBDIRS when all are formatted + +SUBDIRS2 = flightcode +SUBDIRS_FMT = $(SUBDIRS2:%=%_fmt) +SUBDIRS_FMT_DIFF = $(SUBDIRS2:%=%_fmt-diff) + +all: $(SUBDIRS_BUILD) + +build: $(SUBDIRS_BUILD) +$(SUBDIRS_BUILD): + $(MAKE) -C $(@:%_build=%) + +clean: $(SUBDIRS_CLEAN) +$(SUBDIRS_CLEAN): + $(MAKE) -C $(@:%_clean=%) clean + +fmt: $(SUBDIRS_FMT) +$(SUBDIRS_FMT): + $(MAKE) -C $(@:%_fmt=%) fmt + +fmt-diff: $(SUBDIRS_FMT_DIFF) +$(SUBDIRS_FMT_DIFF): + $(MAKE) -C $(@:%_fmt-diff=%) fmt-diff + +.PHONY: $(SUBDIRS) $(SUBDIRS_BUILD) $(SUBDIRS_CLEAN) $(SUBDIRS_FMT) $(SUBDIRS_FMT_DIFF) diff --git a/README.md b/README.md new file mode 100644 index 0000000..b4f44c4 --- /dev/null +++ b/README.md @@ -0,0 +1,15 @@ +WARNING - WORK IN PROGRESS + +``` +This code is known to be broken and/or incomplete. IT DOES NOT WORK. + +We are actively working on fixing it, and we really, really do not recommend you download it just yet. + +We will remove this warning from the repository when it is no longer required. +``` + + +SoloLink +======== + +Software for the SoloLink diff --git a/SOURCE b/SOURCE new file mode 100644 index 0000000..5c7fd4f --- /dev/null +++ b/SOURCE @@ -0,0 +1 @@ +This source code was released by 3DR in the file: sololink_2.2.4_ff71bb8f.tar.gz diff --git a/STM32Loader/checkArtooAndUpdate.py b/STM32Loader/checkArtooAndUpdate.py new file mode 100755 index 0000000..67efa99 --- /dev/null +++ b/STM32Loader/checkArtooAndUpdate.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python + +import glob +import serial +import slip +import os +import subprocess +import sys +import time +import logging +import logging.config +import param_stored_vals_msg # Used for reading stick calibration values +import slip, struct # Used for writing stick calibration values + +sololink_conf = "/etc/sololink.conf" + +ARTOO_SYSINFO_ID = chr(0x3) +ARTOO_UPDATE_ID = chr(0x12) +ARTOO_LOCKOUT_ID = chr(0x13) +ARTOO_CALIBRATE_ID = chr(0x2) + +ARTOO_UPDATE_SUCCESS = chr(1) +ARTOO_UPDATE_FAILED = chr(2) + +ARTOO_LOCKOUT_FALSE = chr(0) +ARTOO_LOCKOUT_TRUE = chr(1) + +ARTOO_BAUD = 115200 + +# update_result should be either ARTOO_UPDATE_SUCCESS or ARTOO_UPDATE_FAILED +def setArtooUpdateComplete(update_result): + ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1) + slipdev = slip.SlipDevice(ser) + slipdev.write("".join([ARTOO_UPDATE_ID, update_result])) + ser.close() + +# lockout should be either ARTOO_LOCKOUT_FALSE or ARTOO_LOCKOUT_TRUE +def setArtooLockout(lockout): + ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1) + slipdev = slip.SlipDevice(ser) + slipdev.write("".join([ARTOO_LOCKOUT_ID, lockout])) + ser.close() + +def mkdir_p(path): + try: + os.makedirs(path) + except OSError: + pass # already exists + +def doUpdateComplete(): + setArtooLockout(ARTOO_LOCKOUT_FALSE) + if not os.path.exists("/log/updates/READY"): + if os.path.exists("/log/updates/UPDATEFAILED"): + f = open("/log/updates/UPDATEFAILED", "r") + # file should be one line; read it all + r = f.read(1000) + f.close() + r = r.strip("\r\n\t\0 ") + logger.info("request \"update failed\" screen (%s)", r) + setArtooUpdateComplete(ARTOO_UPDATE_FAILED) + else: + logger.info("request \"update success\" screen") + setArtooUpdateComplete(ARTOO_UPDATE_SUCCESS) + mkdir_p("/log/updates") + open("/log/updates/READY", "w").close() # "touch" + else: + logger.info("no screen update (READY exists)") + +# return tuple (filename, version), or None +def getFirmwareInfo(): + files = sorted(glob.glob("/firmware/artoo_*.bin")) + if not files: + return None + filename = files[-1] + # Filename may be of the form + # "/firmware/artoo_0.0.0.bin", or + # "/firmware/artoo_v0.0.0.bin". + # Get it without the 'v'. + if filename[16] == 'v': + version = filename[17:-4] + else: + version = filename[16:-4] + return (filename, version) + +# return version as string ("unknown" if can't get version) +def getArtooVersion(): + #Check the version of the stm32 firmware over serial + #The STM32 might be emitting packets already, so we try a few times to get the + #version string. This has been observed to get the version with one retry on + #several occasions. + logger.info("requesting stm32 version") + version = "unknown" + ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1) + slipdev = slip.SlipDevice(ser) + for i in range(5): + slipdev.write("".join([ARTOO_SYSINFO_ID])) + pkt = slipdev.read() + if not pkt: + logger.info("no data received from stm32, retrying") + continue + pkt = "".join(pkt) + if pkt[0] == ARTOO_SYSINFO_ID: + # SysInfo packet is: artoo/src/hostprotocol.cpp + # start size + # 0 1 ARTOO_SYSINFO_ID artoo/src/hostprotocol.h + # 1 12 UniqueId artoo/src/stm32/sys.h + # 13 2 hwversion artoo/src/hostprotocol.cpp + # 15 var Version artoo/src/version.h + # Version may start with an initial 'v', e.g. v0.6.10, + # but we want it starting with the numeric part. + if pkt[15] != 'v': + version = pkt[15:] + else: + version = pkt[16:] + break + logger.info("got %s/%d, retrying", str(hex(ord(pkt[0]))), len(pkt)) + ser.close() + return version + +# Attempt to update the STM32 multiple times +def updateStm32(filename): + # Total attempts will be retry+1, with the last one doing the unprotect. + # Since doing the unprotect results in a support call, retries is set + # such that the unprotect is a last resort. Allow for two init failures + # and two erase failures (truly degenerate) before doing the unprotect + # and (hopefully) fixing a protected stm32. Getting to the unprotect + # (where it is really needed) should happen only once in a controller's + # lifetime, if at all (it should never be needed, really). + stick_cal = None + retry = 4 + while retry >= 0: + if retry > 0: + # don't unprotect chip the first few times an erase fails + logger.info("updating without readout protection disable") + if call_stm32loader(filename, disable_readout_protect=False): + break # Move on if the firmware is successfully loaded. + else: + # last attempt - unprotect chip if erase fails + logger.info("Updating the STM32 has failed several times. We suspect that readout protection is enabled.") + logger.info("Saving Stick Calibration: ") + stick_cal = read_stick_cal() + logger.info(stick_cal) + logger.info("updating with readout protection disable. This will wipe flash.") + call_stm32loader(filename, disable_readout_protect=True) + retry -= 1 + + logger.info("Update complete. Stick cal before update was: {}. (None means it wasn't read before update)".format(stick_cal)) + logger.info("stick cal after update: {}".format(read_stick_cal())) + + # Only write stick calibration if we did disabled readout protection: + if stick_cal is not None: + # Try to write_stick cal multiple times in case it fails. + # This is defensive. I've never seen it fail. + for i in range(3): + write_stick_cal(stick_cal) + time.sleep(1) + logger.info("Stick cal written. Reading it back again as a check:") + new_cal = read_stick_cal() + logger.info(new_cal) + if stick_cal == new_cal: + break + else: + logger.info("Stick cal failed to be written correctly:") + logger.info("Calibration attempted to be written: {}".format(stick_cal)) + logger.info("Calibration found in flash: {}".format(new_cal)) + +# Initiate the stm32loader process to actually update the STM32 chip. Return True if successful. +def call_stm32loader(filename, disable_readout_protect=False): + # subprocess.check_output raises an exception if stm32loader.py returns a nonzero exit code for any reason + # including if readout_protect is enabled. + # Disable readout protect if specified. This will erase all flash. + + if disable_readout_protect: + flags = "-wvqu" + else: + flags = "-wvq" + + try: + s = subprocess.check_output(["stm32loader.py", flags, "-s", "127", + "-b", "115200", "-p", "/dev/ttymxc1", filename], + stderr=subprocess.STDOUT) + logger.info("stm32loader.py returned normally; output:") + success = True + except subprocess.CalledProcessError as cpe: + s = cpe.output + logger.info("stm32loader.py returned error; output:") + success = False + + # this might be ugly, but it gets the output in the log + s = s.strip("\r\n\t\0 ") + logger.info(s) + # Wait a second for the STM32 to come back up before we send it a message later + time.sleep(1) + return success + +def writeVersionFile(version): + f = open("/STM_VERSION", 'w') + f.write(version + '\n') + f.close() + +# return version from /STM_VERSION +def getVersionFile(): + try: + f = open("/STM_VERSION", 'r') + version = f.readline() + f.close() + version = version.strip() + if version == "": + version = "unknown" + except: + version = "unknown" + return version + + +def read_stick_cal(): + # Start the STM32 process because sololink_config sets runlevel to 2 before an update, + # but we need the (runlevel 3) STM32 process to read a stick cal. + p = subprocess.Popen('stm32', shell=True) + time.sleep(3) + + msg = param_stored_vals_msg.fetch() # String containing all artoo params (including stick cal values) + p.terminate() # Kill the STM32 process. + time.sleep(1) # Give time for the STM32 process to die. Not sure if needed. + + # msg is an empty string if the stm32 process is not running (runlevel 2) + if not msg: + logger.info("Could not read stick calibration.") + return None + else: + msg = param_stored_vals_msg.unpack(msg) # Converts string to Dict + axes = msg.get('stickCals') # 6-item list of 3-item tuples + # Pack datastructure into a single list that can be written later + stick_cals = [] + for a in axes: + stick_cals.extend(a) + return stick_cals + + +def write_stick_cal(values): + logger.info("Writing stick cal: {}".format(values)) + + serialpath = '/dev/ttymxc1' + serialport = serial.Serial(serialpath, ARTOO_BAUD) + slipdev = slip.SlipDevice(serialport) + + packed_vals = struct.pack(" /dev/null + if [ ! -e gpio${1} ]; then + echo "${1}" > /sys/class/gpio/export + fi + echo "out" > /sys/class/gpio/gpio${1}/direction + echo "${2}" > /sys/class/gpio/gpio${1}/value + popd > /dev/null +} + +if ${dokill}; then + # stop it + init 2 + # wait for it to be truly gone + until ! killall -q -CONT stm32; do + true + done +fi + +# deassert boot +set_pin ${pin_boot} 0 + +# pulse reset +set_pin ${pin_reset} 1 +sleep ${delay} +set_pin ${pin_reset} 0 + +if ${dokill}; then + # restart everything + init 3 +fi diff --git a/STM32Loader/stm32_check_cal.py b/STM32Loader/stm32_check_cal.py new file mode 100755 index 0000000..a40ea9f --- /dev/null +++ b/STM32Loader/stm32_check_cal.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python + +# stm32loader in /usr/bin is not on the default python path; make +# sure we always find it no matter where this script is installed +import sys +sys.path.append("/usr/bin") +import stm32loader + +stm32loader.QUIET = 0 + +cmd = stm32loader.CommandInterface() +cmd.open("/dev/ttymxc1", 115200) +cmd.initChip() +cmd.cmdGet() +cmd.cmdGetID() + +status = 1 + +msg = "cal is blank" + +try: + data = cmd.readMemory(0x0803f800, 2048) + for c in data: + if c != 255: + msg = "cal is not blank" + status = 0 + break +except: + msg = "error reading cal (readout protect?)" + pass + +cmd.releaseChip() + +print msg + +sys.exit(status) diff --git a/STM32Loader/stm32_readout_protect.py b/STM32Loader/stm32_readout_protect.py new file mode 100755 index 0000000..c8a74c2 --- /dev/null +++ b/STM32Loader/stm32_readout_protect.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python + +# stm32loader in /usr/bin is not on the default python path; make +# sure we always find it no matter where this script is installed +import sys +sys.path.append("/usr/bin") +import stm32loader + +cmd = stm32loader.CommandInterface() +cmd.open("/dev/ttymxc1", 115200) +cmd.initChip() +cmd.cmdGet() +cmd.cmdGetID() +cmd.cmdReadoutProtect() +cmd.releaseChip() diff --git a/STM32Loader/stm32_reset.py b/STM32Loader/stm32_reset.py new file mode 100755 index 0000000..b091659 --- /dev/null +++ b/STM32Loader/stm32_reset.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +# stm32loader in /usr/bin is not on the default python path; make +# sure we always find it no matter where this script is installed +import sys +sys.path.append("/usr/bin") +import stm32loader + +cmd = stm32loader.CommandInterface() +cmd.open("/dev/ttymxc1", 115200) +cmd.initChip() +cmd.cmdGet() +cmd.cmdGetID() +cmd.releaseChip() diff --git a/STM32Loader/stm32_update_test.sh b/STM32Loader/stm32_update_test.sh new file mode 100755 index 0000000..a27826e --- /dev/null +++ b/STM32Loader/stm32_update_test.sh @@ -0,0 +1,149 @@ +#!/bin/bash + +# STM32 Update Testing +# +# forever: +# "update" to factory +# verify calibration +# update to new +# verify calibration +# +# Setup: +# Put factory and latest firmware on controller in /firmware/loaded. +# Copy this script to controller and run it there. +# Set names of those two files in this script (below). +# +# Results go to stdout. Run it like this: +# # ./stm32_update_test.sh 2>&1 | tee -a /log/stm32_test.log & +# to get a log (of both stdout and stderr) and let you disconnect from +# controller while the test runs. If cal is ever detected as missing, +# the test stops. +# +# BIG NOTE: +# +# If you run the script in the background (&), then it is "difficult" +# (I could not figure out how) to stop it without leaving the STM32 in +# a corrupt state (no program to run). If you turn off controller in +# this state, you will have to OPEN IT UP AND FLASH THE STM32 MANUALLY +# to fix it. "Safely" (relative, right) terminate the test like this: +# # ./stm32_update_test.sh 2>&1 | tee -a /log/stm32_test.log & +# [1] 5970 +# # +# (time passes) +# # kill %1 +# (STM32 is mostly likely CORRUPT at this point) +# # mv /firmware/loaded/artoo_1.2.8.bin /firmware +# # checkArtooAndUpdate.py +# (STM32 is flashed) +# +# Little Notes: +# * Stm32 is normally receiving data (mavlink) at the moment the update +# process begins. One thought is that data might somehow remain in the +# uart (imx6 -> STM32), causing problems in init. This test probably +# does not cover that, but another test that *did* catch that shows +# that the effect is to fail to flash artoo - cal is not erased. +# * After killing this test, you may be in runlevel 2, which means the +# controller's power button will NOT work; do an "init 3" then it will +# work. If you control-C, the script will finish the current update +# (up to a minute) and quit, leaving you in runlevel 3. + +factory=artoo_1.2.2.bin +latest=artoo_1.2.8.bin + +# $1 is start address +# $2 is length +# $3 is output file +stm32_read() { + init 2 + sleep 2 + stm32loader.py -q -p /dev/ttymxc1 -r -a $1 -l $2 $3 + sleep 1 + init 3 + sleep 2 +} + +# $1 is the firmware filename +stm32_update() { + cp /firmware/loaded/$1 /firmware + init 2 + sleep 2 + checkArtooAndUpdate.py + init 3 + sleep 2 +} + +check_cal() { + stm32_read 0x0803f800 2048 /log/cal.bin + if cmp -s /log/cal.bin /log/cal-ref.bin; then + return 0 + else + echo "cal read from STM32 does not match reference cal" + echo "reference cal:" + od -x /log/cal-ref.bin + echo "cal read from STM32:" + od -x /log/cal.bin + exit 1 + fi +} + +# check required files are there +if [ ! -f /firmware/loaded/${factory} -o \ + ! -f /firmware/loaded/${latest} ]; then + echo "${factory} or ${latest} not found in /firmware/loaded" + exit 1 +fi + +# get reference cal +echo "reading reference cal..." +stm32_read 0x0803f800 2048 /log/cal-ref.bin +if [ `od -x /log/cal-ref.bin | wc -l` -le 3 ]; then + # cal looks erased already + echo "reference cal already erased?" + od -x /log/cal-ref.bin + exit 1 +fi + +# get erased block (assumes block before cal is erased) +#echo "reading erased block..." +#stm32_read 0x0803f000 2048 /log/cal-bad.bin +#if [ `od -x /log/cal-bad.bin | wc -l` -ne 3 ]; then +# # does not look erased +# echo "erased block isn't erased?" +# od -x /log/cal-bad.bin +# exit 1 +#fi + +# set up so control-C (SIGINT) or kill (SIGTERM) exits between updates +quitting=false + +on_sigint() { + echo "sigint..." + quitting=true +} +trap on_sigint SIGINT + +# The intent is to be able to background this script, then tell it to quit +# with "kill %1" to send it a SIGTERM, but this never gets called... :( +on_sigterm() { + echo "sigterm..." + quitting=true +} +trap on_sigterm SIGTERM + +while true; do + + echo "updating to ${factory}..." + stm32_update ${factory} + echo "checking cal..." + check_cal + + if [ $quitting == "true" ]; then break; fi + + echo "updating to ${latest}..." + stm32_update ${latest} + echo "checking cal..." + check_cal + + if [ $quitting == "true" ]; then break; fi + +done diff --git a/STM32Loader/test_save_stick_cal.sh b/STM32Loader/test_save_stick_cal.sh new file mode 100755 index 0000000..4aecdee --- /dev/null +++ b/STM32Loader/test_save_stick_cal.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +# Purpose of this test + # Enable readout protect and then attempt an artoo update. + # Artoo parameters (i.e. stick cals) should not be wiped. + # Test for IG-1435 + +# Note that this test doesn't always work + # because for some reason I need to run stm32_readout_protect.py a couple times for it to take effect, + # but it should be kept around as inspiration for how to test this functionality manually. + # Testing via a hard power cycle could be more representative than reboot anyway + +# Preconditions for this test: + # Controller is on Solo v2.4.2-1, with Artoo 1.2.11. + # A firmware file for Artoo 1.2.11 named 1.2.22 exists in /firmware/loaded + # stm32_readout_protect.py and this file have been copied from the sololink repo (stm32loader folder) into artoo:/usr/bin + +init 2 # Stop the stm32 process +sleep 5 +cp /firmware/loaded/artoo_1.2.12.bin /firmware/ +python /usr/bin/stm32_readout_protect.py # Enable Readout Protect +echo "Rebooting the controller. Once it comes up, check /log/3dr-solo.log for test results." +sync +reboot \ No newline at end of file diff --git a/STM32Loader/updateArtoo.sh b/STM32Loader/updateArtoo.sh new file mode 100755 index 0000000..f97a7d0 --- /dev/null +++ b/STM32Loader/updateArtoo.sh @@ -0,0 +1,28 @@ +#!/bin/sh + +#Comment out the RCTX line in /etc/inittab +sed -i 's/^RCTX.*/#&/g' /etc/inittab + +#stop the stm32 process +init q +sleep 1 + +# update artoo +# we only erase the first 127 pages since the stm32 stores param data in its last page. +# the stm32 we're currently using has 128 pages of 2k each - this will need to change if we +# ever use different stm32 varieties. +stm32loader.py -vw -s 127 -p /dev/ttymxc1 /home/root/artoo.bin + +# note - verification +# you can read back the last page to verify that it wasn't overwritten in the update process: +# +# ./stm32loader.py -r -a 0x803f800 -l 2048 -p /dev/ttymxc1 readback.bin +# hexdump readback.bin +# +# if it's not all 0xff's you're in good shape! + +#Uncomment the RCTX line +sed -i 's/.\(RCTX*.\)/\1/g' /etc/inittab + +#start the stm32 process back up +init q diff --git a/config/checknet b/config/checknet new file mode 100755 index 0000000..d8747ec --- /dev/null +++ b/config/checknet @@ -0,0 +1,34 @@ +#!/bin/bash + +# Verify network device exists, and reboot if it does not + +# This is a workaround for the case where the PCIe phy does not initialize, +# which happens a few percent of the time. The user-facing effect is that +# startup takes about 20 seconds longer if the phy does not initialize. +# +# It would be better to do this from the initrd. +# +# It would be better still to fix the PCIe driver to reset and try again (or +# something). Simple fixes (e.g. increasing the training time) did not work. +# The imx6 PCIe driver in the 3.14.38 BSP is considerably different from the +# one in the 3.10.17 BSP, so perhaps it will be fixed when we update. + +kmsg() { + echo "$@" > /dev/kmsg +} + +# wlan0 exists on both solo and controller +# (although normally it is not used on controller) +kmsg -n "Checking wlan0... " + +if [ ! -e /sys/class/net/wlan0 ]; then + kmsg "NOT FOUND" + # all logs except boot have been rotated at this point; + # stop and rotate it keep them all in sync + /etc/init.d/bootlogd stop + shutdown -r now + # don't let any more startup scripts start + sleep 60 +else + kmsg "OK" +fi diff --git a/config/configinit b/config/configinit new file mode 100755 index 0000000..cf0a0b5 --- /dev/null +++ b/config/configinit @@ -0,0 +1,285 @@ +#!/bin/bash + +# SoloLink Configuration file validate/restore +# +# Called at init time before any config files are used via an entry in +# /etc/rcS.d. + +cd /etc + +verbose=false + +# arguments +# -v verbose +until [ -z "$1" ]; do + case $1 in + -v) verbose=true;; + esac + shift +done + +dmsg() { + if ${verbose}; then echo "$@"; fi +} + +# Test mode: Ubuntu md5sum is a bit different from busybox md5sum +# /usr/bin/md5sum is a symlink if it points to busybox +if [ -h `which md5sum` ]; then + # assume busybox + # not test mode + MD5SUM="md5sum -cs" + ETC="/etc" + ROFS="/mnt/rootfs.ro" + kmsg() { + echo "$@" > /dev/kmsg + } +else + # assume not busybox; correct for ubuntu but not tested on os-x + # test mode + MD5SUM="md5sum -c --status" + ETC="./etc" + ROFS="./rootfs.ro" + kmsg() { + echo "$@" + } +fi + +kmsg -n "Checking SoloLink configuration... " +dmsg "" + +config_names="hostapd wpa_supplicant sololink shotmanager" + +# For each file "file" in ${config_names}: +# +# A new FS contains only "file.orig" +# +# First boot: +# 1. file.conf does not exist +# 2. file.back does not exist +# 3. copy file.orig to file.base (used for upgrade merge) +# 4. copy file.orig to file.conf +# 5. create file.conf.md5 +# +# Subsequent boots: +# 1. file.conf exists +# 2. file.conf.md5 is good +# +# Program modification of a config file (not in this script): +# 1. copy file.conf to file.back +# 2. create file.back.md5 +# 3. modify file.conf +# 4. create file.conf.md5 +# +# Manual modification of a config file (e.g. via console or ssh): +# 1. edit file.conf +# 2. md5sum > +# +# During the modification process, a shutdown can result in any of the +# following (numbers match completion of steps above): +# 1. file.conf valid, file.back invalid (file.conf is used) +# 2. file.conf valid, file.back valid (file.conf used) +# 3. file.conf invalid, file.back valid (file.conf rollback) +# 4. file.conf valid, file.back valid (file.conf used) +# +# Example recovery: +# 1. file.conf exists +# 2. file.conf.md5 is not good +# 3. file.back exists +# 4. file.back.md5 is good +# 5. copy file.back to file.conf +# 6. create file.conf.md5 +# +# Example upgrade: +# 1. file.conf exists +# 2. file.conf.md5 is good +# 3. file.orig.md5 exists +# 4. file.orig.md5 is not good (because file.orig changed) +# 5. patch = file.base -> file.conf; previous changes (e.g. setting ssid) +# 6. apply patch to new file.orig, creating file.conf +# 7. create file.conf.md5 +# 8. create file.orig.md5 +# 9. copy file.orig to file.base +# 10. create file.base.md5 +# +# Example upgrade (merge fails): +# 1. file.conf exists +# 2. file.conf.md5 is good +# 3. file.orig.md5 exists +# 4. file.orig.md5 is not good (because file.orig changed) +# 5. file.base is not good (does not exist) +# 6. copy file.orig to file.conf +# 7. create file.conf.md5 +# 8. create file.orig.md5 +# 9. copy file.orig to file.base +# 10. create file.base.md5 + +# Return true if file exists, md5 exists, and the md5 is correct +valid() { + [ -f ${1} ] && [ -f ${1}.md5 ] && ${MD5SUM} ${1}.md5 +} + +# Return true if orig.md5 does not exist, or it exists but is not correct +upgraded() { + [ ! -f ${1}.orig.md5 ] || ! ${MD5SUM} ${1}.orig.md5 +} + +# Create backup of .conf +do_backup() { + dmsg "backup ${1}.conf -> ${1}.back" + cp ${1}.conf ${1}.back + md5sum `realpath ${1}.back` > ${1}.back.md5 +} + +# Restore one file to another, i.e. .back or .orig to .conf +do_restore() { + dmsg "copy ${1} -> ${2}" + cp ${1} ${2} + md5sum `realpath ${2}` > ${2}.md5 +} + +# Note that the busybox 'patch' we are using is relatively limited: +# patch [-p NUM] [-i DIFF] [-R] [-N] +# -p NUM Strip NUM leading components from file names +# -i DIFF Read DIFF instead of stdin +# -R Reverse patch +# -N Ignore already applied patches +# +# Busybox patch's return status is not documented. GNU patch's return is this: +# patch's exit status is 0 if all hunks are applied successfully, 1 if some +# hunks cannot be applied or there were merge conflicts, and 2 if there is +# more serious trouble. +# We assume here it is zero on success, nonzero on any failure. +do_patch() { + #dmsg "patch ${1}.conf" + patch ${1}.conf < ${1}.patch + # patch's return code is this function's return code +} + +# Merge changes. +# +# config.base is a copy of the previous config.orig (before this update) +# config.conf is that, plus any changes the user has made +# config.orig is the _new_ config for this update +# +# 1. config.base -> config.conf is what has changed from the previous +# config.orig to the previous config.conf. Create a patch from that. +# +# 2. Attempt to apply the patch to the _new_ config.orig. +# +# a. If the patch applies cleanly, the new config.conf is the new +# config.orig plus patch. +# +# b. If the patch does not apply cleanly, the new config.conf is just +# copied from the new config.orig (changes are lost). +# +# Argument is the config file name root, e.g. "hostapd" means we are working +# with hostapd.base, hostapd.conf, and hostapd.orig. +# +do_merge() { + dmsg "merge ${1}" + + # config.conf is known to be valid at this point + + if valid ${1}.base; then + # config.base is valid; try to merge changes into new config + + # config.patch is the user's changes in the pre-upgrade system + diff -au ${1}.base ${1}.conf > ${1}.patch + + # start with upgraded config.conf... + do_backup ${1} + cp ${1}.orig ${1}.conf + + # apply user's changes + if ! do_patch ${1}; then + # some hunks failed - partial patching seems confusing, so revert it all + dmsg "patch ${1}.conf failed" + dmsg "copy ${1}.orig -> ${1}.conf" + cp ${1}.orig ${1}.conf + fi + + # .conf is good now, either updated or just copied + md5sum `realpath ${1}.conf` > ${1}.conf.md5 + rm -f ${1}.patch ${1}.back* ${1}.conf.orig + + else + # config.base is not valid, can't merge + dmsg "${1}.base not valid; can't merge" + dmsg "copy ${1}.orig -> ${1}.conf" + cp ${1}.orig ${1}.conf + md5sum `realpath ${1}.conf` > ${1}.conf.md5 + fi + + # this is how we detect when the next upgrade happens + md5sum `realpath ${1}.orig` > ${1}.orig.md5 + + # new config.base will be used in the next upgrade + cp ${1}.orig ${1}.base + md5sum `realpath ${1}.base` > ${1}.base.md5 +} + +# Loop over all config files, validating and perhaps restoring each one +for config_name in ${config_names}; do + config=${ETC}/${config_name} + + # skip any that are not in the read-only FS (e.g. shotmanager on controller) + if [ ! -f ${ROFS}/etc/${config_name}.orig ]; then + continue + fi + + dmsg "" + dmsg "${config_name}... " + + if valid ${config}.conf; then + dmsg "${config}.conf is valid" + + if upgraded ${config}; then + kmsg -n "merging ${config}... " + dmsg "" + # typical just after an upgrade if .orig file changes + do_merge ${config} + else + # typical startup + dmsg "done" + fi + + elif valid ${config}.back; then + dmsg "${config}.back is valid" + + # previous shutdown corrupted the .conf file + kmsg -n "restoring ${config}.conf... " + dmsg "" + do_restore ${config}.back ${config}.conf + if upgraded ${config}; then + kmsg -n "merging ${config}... " + dmsg "" + # edge case: previous shutdown prepared update and corrupted .conf file + do_merge ${config} + else + dmsg "done" + fi + + else + dmsg "neither ${config}.conf nor ${config}.back is valid" + + # typical on first boot + kmsg -n "initializing ${config}.conf... " + dmsg "" + if [ ! -f ${config}.orig ]; then + kmsg -n "${config}.orig not found; retrieving from ${ROFS}/etc/${config_name}.orig... " + dmsg "" + cp ${ROFS}/etc/${config_name}.orig ${config}.orig + fi + dmsg "copy ${config}.orig -> ${config}.base" + cp ${config}.orig ${config}.base + md5sum `realpath ${config}.base` > ${config}.base.md5 + do_restore ${config}.orig ${config}.conf + md5sum `realpath ${config}.orig` > ${config}.orig.md5 + dmsg "done" + + fi + +done + +dmsg "" +kmsg "OK" diff --git a/config/logrotate-sololink.conf.controller b/config/logrotate-sololink.conf.controller new file mode 100644 index 0000000..c8c38b5 --- /dev/null +++ b/config/logrotate-sololink.conf.controller @@ -0,0 +1,23 @@ +# see "man logrotate" for details +# rotate logs, keeping 20 versions + +# specifying 'create' makes it so the log file is always there, even if +# nothing writes to it on a particular run, so on the next rotation, the empty +# file is rotated along with all the others, keeping the .N extensions in sync +# (e.g. so all the .2 files are from the same run). + +# specifying 'missingok' is needed for the very first run when there are no +# log files to rotate. + +create +missingok +rotate 20 +nodateext + +/log/3dr-top.log { } +/log/3dr-solo.log { } +/log/3dr-stm32.log { } +/log/3dr-video.log { } +/log/3dr-wifi.log { } +/log/3dr-temp.log { } +/log/pkt_delays.csv { } diff --git a/config/logrotate-sololink.conf.solo b/config/logrotate-sololink.conf.solo new file mode 100644 index 0000000..f53edc1 --- /dev/null +++ b/config/logrotate-sololink.conf.solo @@ -0,0 +1,25 @@ +# see "man logrotate" for details +# rotate logs, keeping 20 versions + +# specifying 'create' makes it so the log file is always there, even if +# nothing writes to it on a particular run, so on the next rotation, the empty +# file is rotated along with all the others, keeping the .N extensions in sync +# (e.g. so all the .2 files are from the same run). + +# specifying 'missingok' is needed for the very first run when there are no +# log files to rotate. + +create +missingok +rotate 20 +nodateext + +/log/3dr-top.log { } +/log/3dr-solo.log { } +/log/3dr-rc.log { } +/log/3dr-telem.log { } +/log/3dr-wifi.log { } +/log/3dr-video.log { } +/log/3dr-temp.log { } +/log/shotlog.log { } +/log/3dr-accessorykit.log { } diff --git a/config/max_dgram_qlen b/config/max_dgram_qlen new file mode 100755 index 0000000..f487ab0 --- /dev/null +++ b/config/max_dgram_qlen @@ -0,0 +1,3 @@ +#! /bin/sh +echo "100" > /proc/sys/net/unix/max_dgram_qlen +echo "max_dgram_qlen=`cat /proc/sys/net/unix/max_dgram_qlen`" diff --git a/config/sololink b/config/sololink new file mode 100644 index 0000000..434e7e0 --- /dev/null +++ b/config/sololink @@ -0,0 +1,5 @@ +# This file goes in /etc/profile.d/ and is sourced by /etc/profile at boot time +# Contents are created by sololink recipe, and should end up looking something +# like this: +# export SOLOLINK_CONFIG_DIR=/etc +# export SOLOLINK_LOG_DIR=/log diff --git a/config/sololink.orig b/config/sololink.orig new file mode 100644 index 0000000..030bcde --- /dev/null +++ b/config/sololink.orig @@ -0,0 +1,191 @@ +[solo] + +artooIp=10.1.1.1 + +soloIp=10.1.1.10 + +# Address to which RC packets are sent +rcDestPort=5005 + +# Address to which 'sysinfo' packet for the STM32 is sent +sysDestIp=%(artooIp)s +sysDestPort=5012 + +# Port to which 'pair request' packet for the STM32 is sent +pairReqDestPort=5013 + +# Port to which 'pair result' packet for the STM32 is sent +pairResDestPort=5014 + +# Port to which MAVLink packets for the STM32 are sent +mavDestPort=5015 + +# Port to connect to for button events (TCP) +buttonEventPort=5016 + +# Port to send button function config messages to +buttonFunctionConfigDestPort=5017 + +# Port to send set shot info messages to +setShotInfoDestPort=5018 + +# Port to send updater messages to +updaterDestPort=5019 + +# Port to which MAVLink packets are sent for all external systems +telemDestPort=14550 + +# TCP port where app server listens for connections +appServerPort=5502 + +# File where app_server saves connect app's IP address +appAddressFile=/var/run/solo_app.ip + +# Artoo's serial ports + +# Console is /dev/ttymxc0 + +# STM32 +stm32Dev=/dev/ttymxc1 +stm32Baud=115200 + +# Solo's serial ports + +# Console is /dev/ttymxc0 + +# Pixhawk telemetry +telemDev=/dev/ttymxc1 +telemBaud=921600 +telemFlow=True + +# Telemetry logging control +telemLogGap=1000000 +telemLogDelayMax=100000 +#telemLogDelayFile=/tmp/pkt_delays.csv + +# Pixhawk RC +rcDsmDev=/dev/ttymxc2 +rcDsmBaud=115200 + +# IP addresses from which Solo accepts RC packets +rcSourceIps=10.1.1.1,127.0.0.1 + +# Set system time from GPS when available +useGpsTime=True + +# Throttle PWM mapping +pwmInMinThrottle=1000 +pwmInMaxThrottle=2000 +pwmOutMinThrottle=1000 +pwmOutMaxThrottle=1900 + +# Rc timeout max +rcTimeoutUS=400000 + +# Telemetry display units (metric or imperial) +uiUnits=metric + +[pairing] +user_confirmation_timeout = 30.0 +controller_link_port = 5501 +wifi_connect_timeout = 5.0 +connect_request_interval = 1.0 +connect_ack_timeout = 0.5 +solo_address_file = /var/run/solo.ip +button_filename = /dev/input/event0 + +[net] +ApEnable=True +StationEnable=False + +[loggers] +keys=root,stm32,pix,pair,net,app,tlm,shot + +[handlers] +keys=consoleHandler,sysLogHandler,sysLog2Handler + +[formatters] +keys=simpleFormatter,syslogFormatter + +[logger_root] +level=INFO +handlers=consoleHandler + +[logger_stm32] +level=INFO +handlers=sysLogHandler +qualname=stm32 +propagate=0 + +[logger_pix] +level=INFO +handlers=sysLogHandler +qualname=pix +propagate=0 + +[logger_pair] +level=INFO +handlers=sysLogHandler +qualname=pair +propagate=0 + +[logger_net] +level=INFO +handlers=sysLogHandler +qualname=net +propagate=0 + +[logger_app] +level=INFO +handlers=sysLogHandler +qualname=app +propagate=0 + +[logger_tlm] +level=INFO +handlers=sysLogHandler +qualname=tlm +propagate=0 + +[logger_shot] +level=INFO +handlers=sysLog2Handler +qualname=shot +propagate=0 + +[handler_consoleHandler] +class=StreamHandler +level=ERROR +formatter=simpleFormatter +args=(sys.stdout,) + +[handler_sysLogHandler] +class=handlers.SysLogHandler +level=DEBUG +formatter=syslogFormatter +args=("/dev/log", handlers.SysLogHandler.LOG_LOCAL1) + +[handler_sysLog2Handler] +class=handlers.SysLogHandler +level=DEBUG +formatter=syslogFormatter +args=("/dev/log", handlers.SysLogHandler.LOG_LOCAL2) + +[formatter_simpleFormatter] +format=%(asctime)s %(name)-4s %(levelname)-8s %(message)s +datefmt= + +[formatter_syslogFormatter] +format=%(name)s: %(message)s +datefmt= + +[video] +videoMinFR=24 +videoMaxFR=24 +videoMinBR=800000 +videoMaxBR=1800000 +videoFRStep=5 +videoBRStep=100000 +varStreamRes=True +cropRecordRes=True + diff --git a/config/sololink_config b/config/sololink_config new file mode 100755 index 0000000..5dc67d8 --- /dev/null +++ b/config/sololink_config @@ -0,0 +1,1289 @@ +#!/bin/bash + +# set SOLOLINK_CONFIG_TEST for testing (don't shutdown etc.) +# set ROOT if config files are in a test directory (not at /) +# set TESTHOSTNAME and it will be used instead of the real host name + +prog_name="sololink_config" + +# We allow the version of this file to deviate from that of SoloLink. It may +# make sense in some cases to download a new version of this script to bring +# it up to date, then do other operations that might require the latest +# version. +sololink_config_version="0.0.1" + +logger -p local1.info -- $0 $* + +if [ -h `which md5sum` ]; then + # assume busybox + MD5SUM="md5sum -s" +else + # assume not busybox; correct for ubuntu but not tested on os-x + MD5SUM="md5sum --status" +fi + + +usage() { +echo " +Usage: ${prog_name} OPTION [OPTION_ARG] + +Options: + --get-wifi-ssid + print wifi SSID + --set-wifi-ssid SSID [--force] + --get-wifi-password + print wifi password + --set-wifi-password PASSWORD + --get-wifi-channel + print wifi channel + --set-wifi-channel CHANNEL + --get-wifi-country + print wifi country (regulatory domain) + --set-wifi-country COUNTRY + set wifi country (regulatory domain) + --get-wifi-power + --set-wifi-power POWER + --reset-wifi-settings + controller: revert to default SSID (on next boot) + solo: forget SSID (unpair) + --restart-video + solo: Restart the video pipeline + --get-ui-mode + print tx mode, "1" or "2" + --set-ui-mode MODE + change tx mode, MODE = 1 or 2 + --get-ui-units + print display units, "metric" or "imperial" + --set-ui-units UNITS + change display units, UNITS = "metric" or "imperial" + --get-ui-sweep + print camera tilt sweep times, minimum then maximum (seconds) + --set-ui-sweep MIN MAX + change camera tilt sweep time limits, MIN and MAX in seconds + --factory-reset + do a factory reset + --settings-reset + do a settings reset + --make-golden + make 'latest' the new 'golden' + --update-prepare artoo|pixhawk|sololink + --update-apply artoo|pixhawk|sololink [--force] [--reset] + --get-image + prints 'golden' or 'latest' + --get-pairing + controller: prints MAC of paired solo + solo: prints SSID of paired controller + --set-pairing [PEER] + --get-config VARNAME + print value of VARNAME in sololink.conf + --set-config VARNAME VALUE + set VARNAME in sololink.conf to VALUE + --get-status + get overall system status + --get-version [artoo|pixhawk|sololink|golden] + --restart [video|sololink|system] + --reboot + --pass + prints nothing, exit status 0 + --fail + prints nothing, exit status 1 + +Exit status: + 0 if command succeeded + 1 if command failed + +Script version ${sololink_config_version} +" +exit 1 +} + + +# shutdown (unless testing) +shutdown_now() { + if [ -z "${SOLOLINK_CONFIG_TEST}" ]; then + shutdown -r now + fi +} + + +# get current runlevel +get_runlevel() { + rl=`runlevel` + for w in ${rl}; do + a=${w} + done + echo ${a} +} + + +# backup_config [path/basename] +# e.g. backup_config /etc/hostapd +backup_config() { + cp ${1}.conf ${1}.back + md5sum ${1}.back > ${1}.back.md5 + sync +} + + +# md5sum_config [path/basename] +# e.g. md5sum_config /etc/hostapd +md5sum_config() { + md5sum ${1}.conf > ${1}.conf.md5 + sync + rm -f ${1}.back ${1}.back.md5 +} + + +# return number of files in a directory matching a glob pattern +# count_files (DIR) (PATTERN) +count_files() { + if [ ! -d ${1} ]; then + echo "0" + else + find ${1} -name ${2} | wc -l + fi +} + + +# set network in wpa_supplicant +# set_wpa_supplicant_network (SSID) (PASSWORD) +set_wpa_supplicant_network() { + backup_config ${ROOT}/etc/wpa_supplicant + cp ${ROOT}/mnt/rootfs.ro/etc/wpa_supplicant.orig ${ROOT}/etc/wpa_supplicant.conf + echo >> ${ROOT}/etc/wpa_supplicant.conf + echo "network={" >> ${ROOT}/etc/wpa_supplicant.conf + echo " ssid=\"${1}\"" >> ${ROOT}/etc/wpa_supplicant.conf + echo " psk=\"${2}\"" >> ${ROOT}/etc/wpa_supplicant.conf + echo " proto=RSN" >> ${ROOT}/etc/wpa_supplicant.conf + echo " key_mgmt=WPA-PSK" >> ${ROOT}/etc/wpa_supplicant.conf + echo " pairwise=CCMP" >> ${ROOT}/etc/wpa_supplicant.conf + echo " auth_alg=OPEN" >> ${ROOT}/etc/wpa_supplicant.conf + echo "}" >> ${ROOT}/etc/wpa_supplicant.conf + md5sum_config ${ROOT}/etc/wpa_supplicant +} + + +# return 0 (true) if wpa_supplicant.conf has an ssid= setting +# return nonzero (false) otherwise +wpa_supp_has_ssid() { + grep -q ssid= ${ROOT}/etc/wpa_supplicant.conf +} + + +# return 0 (true) if wpa_supplicant.conf has a psk= setting +# return nonzero (false) otherwise +wpa_supp_has_psk() { + grep -q psk= ${ROOT}/etc/wpa_supplicant.conf +} + + +reset_wifi_settings() { + if [ ${is_solo} ]; then + backup_config ${ROOT}/etc/wpa_supplicant + cp ${ROOT}/mnt/rootfs.ro/etc/wpa_supplicant.orig ${ROOT}/etc/wpa_supplicant.conf + md5sum_config ${ROOT}/etc/wpa_supplicant + else + backup_config ${ROOT}/etc/hostapd + cp ${ROOT}/mnt/rootfs.ro/etc/hostapd.orig ${ROOT}/etc/hostapd.conf + md5sum_config ${ROOT}/etc/hostapd + fi + return 0 +} + + +get_wifi_password() { + if [ ${is_solo} ]; then + # is there one? + if ! wpa_supp_has_psk; then + echo "wifi network not set" + return 1 + fi + password=`grep psk= ${ROOT}/etc/wpa_supplicant.conf | head -n 1 | \ + sed "s/.*psk=\"\(.\+\)\"/\1/g"` + else + # there is always one in hostapd + password=`grep ^wpa_passphrase= ${ROOT}/etc/hostapd.conf | head -n 1 | \ + sed "s/^wpa_passphrase=\(.\+\)\$/\1/g"` + fi + echo "${password}" + return 0 +} + + +set_wifi_password() { + # password must be 8..63 characters + pwlen=${#1} + if [ ${pwlen} -lt 8 -o ${pwlen} -gt 63 ]; then + echo "password must be 8..63 characters" + return 1 + fi + if [ ${is_solo} ]; then + # is there one already? + if ! wpa_supp_has_psk; then + # make a default network and set password + set_wpa_supplicant_network "SoloLink_Default" "${1}" + else + backup_config ${ROOT}/etc/wpa_supplicant + sed -i "s/psk=\".\+\"/psk=\"${1}\"/g" ${ROOT}/etc/wpa_supplicant.conf + md5sum_config ${ROOT}/etc/wpa_supplicant + fi + else + backup_config ${ROOT}/etc/hostapd + sed -i "s/^wpa_passphrase=.\+\$/wpa_passphrase=${1}/g" ${ROOT}/etc/hostapd.conf + md5sum_config ${ROOT}/etc/hostapd + fi + get_wifi_password +} + + +get_wifi_ssid_wpa_supplicant() { + echo `grep ssid= ${ROOT}/etc/wpa_supplicant.conf | head -n 1 | \ + sed "s/.*ssid=\"\(.\+\)\"/\1/g"` +} + + +get_wifi_ssid_hostapd() { + echo `grep ^ssid= ${ROOT}/etc/hostapd.conf | head -n 1 | \ + sed "s/^ssid=\(.\+\)\$/\1/g"` +} + + +get_wifi_ssid() { + if [ ${is_solo} ]; then + # is there one? + if ! wpa_supp_has_ssid; then + echo "wifi network not set" + return 1 + fi + ssid=`get_wifi_ssid_wpa_supplicant` + else + ssid=`get_wifi_ssid_hostapd` + fi + echo "${ssid}" + return 0 +} + + +validate_wifi_ssid() { + # ssid must start with "SoloLink_" and be no more than 32 characters total + ssidlen=${#1} + if [ ${ssidlen} -lt 9 -o ${ssidlen} -gt 32 -o \ + "`expr substr "${1}" 1 9`" != "SoloLink_" ]; then + echo "ssid must start with \"SoloLink_\" and be no more that 32 characters long" + return 1 + fi + return 0 +} + + +set_wifi_ssid_wpa_supplicant() { + backup_config ${ROOT}/etc/wpa_supplicant + sed -i "s/ssid=\".\+\"/ssid=\"${1}\"/g" ${ROOT}/etc/wpa_supplicant.conf + md5sum_config ${ROOT}/etc/wpa_supplicant + return 0 +} + + +set_wifi_ssid_hostapd() { + backup_config ${ROOT}/etc/hostapd + sed -i "s/^ssid=.\+\$/ssid=${1}/g" ${ROOT}/etc/hostapd.conf + md5sum_config ${ROOT}/etc/hostapd + return 0 +} + + +set_wifi_ssid() { + if ! validate_wifi_ssid "${1}"; then + return 1 + fi + if [ ${is_solo} ]; then + # is there one already? + if ! wpa_supp_has_ssid; then + # make a default network and set ssid + set_wpa_supplicant_network "${1}" "sololink" + else + set_wifi_ssid_wpa_supplicant "${1}" + fi + else + set_wifi_ssid_hostapd "${1}" + fi + get_wifi_ssid +} + +get_wifi_channel() { + if [ ${is_solo} ]; then + echo "wifi channel not accessible on solo" + return 1 + fi + channel=`iw wlan0-ap info | grep channel | awk '{print $2}'` + echo "${channel}" + return 0 +} + +set_wifi_channel() { + # validate that the channel is between 1 and 11 + desiredChannel="${1}" + if [[ $desiredChannel =~ ^-?[0-9]+$ ]]; then + if [[ "$desiredChannel" -lt 1 || "$desiredChannel" -gt 11 ]]; then + echo "error: wifi channel must be between 1 and 11" + return 1 + fi + else + echo "error: attempting to set wifi channel to non-integer" + return 1 + fi + + baseFreq=2412 + channelMult=`expr $desiredChannel - 1` + freqAdd=`expr $channelMult \* 5` + freq=`expr $baseFreq + $freqAdd` + echo "$desiredChannel" + hostapd_cli chan_switch 20 ${freq} ht + return 0 +} + +get_wifi_country() { + if [ ${is_solo} ]; then + echo "wifi country can only be retrieved on controller" + return 1 + fi + country=`grep ^country_code= ${ROOT}/etc/hostapd.conf | head -n 1 | \ + sed "s/^country_code=\(.\+\)\$/\1/g"` + echo "${country}" + return 0 +} + +country_list="AD,AE,AF,AI,AL,AM,AN,AR,AT,AU,AW,AZ,BA,BB,BD,BE,BF,BG,BH,BL,BN,BO,BR,BT,BY,BZ,CA,CF,CH,CI,CL,CN,CO,CR,CY,CZ,DE,DK,DM,DO,DZ,EC,EE,EG,ES,FI,FM,FR,GE,GB,GD,GH,GR,GL,GT,GU,GY,HN,HK,HR,HT,HU,ID,IE,IL,IN,IS,IR,IT,JM,JP,JO,KE,KH,KN,KP,KR,KW,KZ,LB,LC,LI,LK,LS,LT,LU,LV,MC,MA,MD,ME,MF,MO,MH,MK,MR,MT,MY,MX,NL,NO,NP,NZ,OM,PA,PE,PG,PH,PK,PL,PM,PT,PR,PW,QA,RO,RS,RU,RW,SA,SE,SG,SI,SK,SN,SV,SY,TC,TD,TG,TW,TH,TT,TN,TR,UA,US,UY,UZ,VC,VE,VN,VU,WF,YE,ZA,ZW" + +set_wifi_country() { + if [ ${is_solo} ]; then + echo "wifi country can only be set on controller" + return 1 + fi + # country is two uppercase characters + if [[ ! ${1} =~ ^[A-Z]{2}$ ]]; then + echo "country must be 2 uppercase characters" + return 1 + fi + if [[ ! ${country_list} =~ ${1} ]]; then + echo "country is not in the list of valid country codes" + return 1 + fi + backup_config ${ROOT}/etc/hostapd + sed -i "s/^country_code=.\+\$/country_code=${1}/g" ${ROOT}/etc/hostapd.conf + md5sum_config ${ROOT}/etc/hostapd + return 0 +} + +get_wifi_power() { + if [ -z /sys/kernel/debug/ieee80211/phy0/power ]; then + echo "error: unable to read power level" + return 1 + fi + + power=`cat /sys/kernel/debug/ieee80211/phy0/power` + if [ $power -gt 27 ]; then power=27; fi + echo "$power" + return 0 +} + +set_wifi_power() { + desiredPower="${1}" + if [[ $desiredPower =~ ^-?[0-9]+$ ]]; then + if [[ "$desiredPower" -lt 0 || "$desiredPower" -gt 27 ]]; then + echo "error: wifi power level must be between 0 and 27" + return 1 + fi + else + echo "error: attempting to set wifi power to non-integer" + return 1 + fi + + echo "$desiredPower" + iw phy0 set txpower fixed $((desiredPower*100)) + return 0 +} + +# Video pipeline modification parameters +restart_video() { + if [ ${is_controller} ]; then + echo "video can only be restarted on solo" + return 1 + fi + + killall vidlaunch + echo "restarted video" + return 0 +} + +# ui_mode is the transmitter mode (1 or 2) +# +# Persisted in the STM32 flash as a "stick mapping", where the stick mapping +# corresponds to a mode 1 or mode 2 layout. ui_mode is set using a python +# program that writes a stick mapping. This script translates the '1' or '2' +# requested mode to a config file, then gives that config file to the python +# program. To set the mode, the python program writes the stick mapping to +# the STM32. To read the mode, the python reads the current stick mapping from +# the STM32 and compares it to a specified config file, so to read the mode, +# we are asking the python program if the current mode matches the mode 1 +# config file, and if not, if it matches the mode 2 config file. + +ui_mode_prog="config_stick_axes.py" + +# arg is normally mode1, mode2, or default +ui_mode_config_file() { + echo "/firmware/cfg/stick-cfg-evt-${1}.cfg" +} + +get_ui_mode() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only get mode on controller" + return 1 + fi + # is it still defaults? + cf=`ui_mode_config_file default` + if [ `${ui_mode_prog} get ${cf}` == "true" ]; then + # default is mode 2 (artoo firmware) + echo "2" + return 0 + fi + # is it mode 1? + cf=`ui_mode_config_file mode1` + if [ `${ui_mode_prog} get ${cf}` == "true" ]; then + echo "1" + return 0 + fi + # is it mode 2? + cf=`ui_mode_config_file mode2` + if [ `${ui_mode_prog} get ${cf}` == "true" ]; then + echo "2" + return 0 + fi + # neither 1 nor 2! + echo "mode unknown" + return 0 +} + +set_ui_mode() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only set mode on controller" + return 1 + fi + # argument required + if [ -z "${1}" ]; then + echo "sololink_config --set-ui-mode [1|2]" + return 1 + fi + # argument must be 1 or 2 + if [ "${1}" != "1" -a "${1}" != "2" ]; then + echo "sololink_config --set-ui-mode [1|2]" + return 1 + fi + # send to STM32 + ${ui_mode_prog} set `ui_mode_config_file mode${1}` + # echo new mode to stdout + echo "${1}" + return 0 +} + +# ui_units is "metric" or "imperial" +# +# Not persisted in the STM32, and can't be read back, so the last known +# setting is saved in sololink.conf. ui_units is set in the STM32 on each +# boot. + +get_ui_units() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only set units on controller" + return 1 + fi + # get it from config file + if grep -q uiUnits= ${ROOT}/etc/sololink.conf; then + echo "`grep uiUnits= ${ROOT}/etc/sololink.conf | head -n 1 | \ + sed "s/.*uiUnits\s*=\(.\+\)/\1/g"`" + else + # no setting; default is metric + echo "metric" + fi + return 0 +} + +set_ui_units() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only set units on controller" + return 1 + fi + # argument required + if [ -z "${1}" ]; then + echo "sololink_config --set-ui-units [metric|imperial]" + return 1 + fi + # argument must be metric or imperial + if [ "${1}" != "metric" -a "${1}" != "imperial" ]; then + echo "sololink_config --set-ui-units [metric|imperial]" + return 1 + fi + # do it + backup_config ${ROOT}/etc/sololink + if grep -q uiUnits= ${ROOT}/etc/sololink.conf; then + # change existing setting + sed -i "s/uiUnits\s*=.\+/uiUnits=${1}/g" ${ROOT}/etc/sololink.conf + else + # append as new setting + echo >> ${ROOT}/etc/sololink.conf + echo "uiUnits=${1}" >> ${ROOT}/etc/sololink.conf + fi + md5sum_config ${ROOT}/etc/sololink + # action! + set_ui_units_prog="set_telem_units.py" + set_ui_units_args="${1}" + if which ${set_ui_units_prog} &> /dev/null; then + ${set_ui_units_prog} ${set_ui_units_args} + fi + # echo new units to stdout + echo "${1}" + return 0 +} + +# ui_sweep is the min/max range for the camera tilt sweep dial +# +# Persisted in the STM32. A python program reads it from or writes it to the +# STM32 on request. + +get_ui_sweep() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only get sweep on controller" + return 1 + fi + # fetch from STM32 - prints "min max" to stdout + config_sweep_time.py + return 0 +} + +set_ui_sweep() { + # meaningful only on controller + if [ ${is_solo} ]; then + echo "can only set sweep on controller" + return 1 + fi + # two arguments required: min max + if [ -z "${1}" -o -z "${2}" ]; then + echo "sololink_config --set-ui-sweep " + return 1 + fi + # not done: check that arguments are integers + # send to STM32 - this echoes new setting to stdout + config_sweep_time.py ${1} ${2} + return 0 +} + +factory_reset() { + rm -rf ${ROOT}/log/updates + mkdir -p ${ROOT}/log/updates + touch ${ROOT}/log/updates/FACTORYRESET + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py start + fi + shutdown_now + return 0 +} + + +settings_reset() { + rm -rf ${ROOT}/log/updates + mkdir -p ${ROOT}/log/updates + touch ${ROOT}/log/updates/RESETSETTINGS + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py start + fi + shutdown_now + return 0 +} + + +make_golden() { + if [ ! -z "${SOLOLINK_CONFIG_TEST}" ]; then + echo "no test mode for make_golden" + return 1 + fi + image=`get_image` + if [ ${image} == "golden" ]; then + echo "already running golden" + return 1 + elif [ ${image} != "latest" ]; then + echo "${image}" + return 1 + fi + umount /dev/mmcblk0p1 &> /dev/null + mkfs.vfat /dev/mmcblk0p1 -n GOLDEN &> /dev/null + mkdir -p ${ROOT}/tmp/golden + mount /dev/mmcblk0p1 ${ROOT}/tmp/golden + cp -r /mnt/boot/* ${ROOT}/tmp/golden + umount ${ROOT}/tmp/golden + return 0 +} + + +update_prepare() { + case "${1}" in + "artoo" ) + if [ ${is_solo} ]; then + echo "solo can't update artoo" + return 1 + fi + rm -rf ${ROOT}/mnt/rootfs.rw/firmware + mkdir -p ${ROOT}/firmware + ;; + "pixhawk" ) + if [ ! ${is_solo} ]; then + echo "controller can't update pixhawk" + return 1 + fi + rm -rf ${ROOT}/mnt/rootfs.rw/firmware + mkdir -p ${ROOT}/firmware + ;; + "sololink" ) + rm -rf ${ROOT}/log/updates + mkdir -p ${ROOT}/log/updates + rm -rf ${ROOT}/mnt/rootfs.rw/firmware + mkdir -p ${ROOT}/firmware + ;; + * ) + echo "--update-prepare [artoo|pixhawk|sololink]" + return 1 + ;; + esac + return 0 +} + + +update_apply_artoo() { + # check update file unless --force was given + #if [ "${1}_" != "--force_" ]; then + # sanity-check whatever is in /firmware/artoo_*.bin + #fi + if [ -z "${SOLOLINK_CONFIG_TEST}" ]; then + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py start + fi + r=`get_runlevel` + if [ "${r}_" != "2_" ]; then + init 2 + sleep 3 + fi + checkArtooAndUpdate.py + if [ "${r}_" != "2_" ]; then + init 3 + fi + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py success + fi + fi + return 0 +} + + +update_apply_pixhawk() { + # check update file unless --force was given + #if [ "${1}_" != "--force_" ]; then + # sanity-check whatever is in /firmware/*.px4 + #fi + if [ -z "${SOLOLINK_CONFIG_TEST}" ]; then + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py start + fi + r=`get_runlevel` + if [ "${r}_" != "2_" ]; then + init 2 + sleep 3 + fi + pixhawk.py -i + if [ "${r}_" != "2_" ]; then + init 3 + fi + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py success + fi + fi + return 0 +} + + +update_apply_sololink() { + # check update file unless --force was given + if [ "${1}_" != "--force_" ]; then + # must be only one update file, and must match either + # *controller*.tar.gz or *solo*.tar.gz + if [ ${is_solo} ]; then + file_infix="solo" + else + file_infix="controller" + fi + n=`count_files "${ROOT}/log/updates" "*${file_infix}*.tar.gz"` + if [ ${n} -ne 1 ]; then + echo "must be exactly one update file (found ${n})" + return 1 + fi + n=`count_files "${ROOT}/log/updates" "*${file_infix}*.tar.gz.md5"` + if [ ${n} -ne 1 ]; then + echo "must be exactly one md5 file (found ${n})" + return 1 + fi + # check md5 + pushd ${ROOT}/log/updates &> /dev/null + if ! ${MD5SUM} -c *${file_infix}*.tar.gz.md5; then + echo "md5 error" + return 1 + fi + popd &> /dev/null + fi + + # nasty cleanup + rm -f \ + ${ROOT}/mnt/rootfs.rw/etc/inittab \ + ${ROOT}/mnt/rootfs.rw/*VERSION* \ + ${ROOT}/log/updates/FACTORYRESET \ + ${ROOT}/log/updates/RESETSETTINGS + touch ${ROOT}/log/updates/UPDATE + if [ "${1}_" == "--reset_" ] || [ "${2}_" == "--reset_" ]; then + touch ${ROOT}/log/updates/RESETSETTINGS + fi + if [ -x ${ROOT}/usr/bin/updater_msg.py ]; then + ${ROOT}/usr/bin/updater_msg.py start + fi + shutdown_now +} + + +update_apply() { + case "${1}" in + "artoo" ) + if [ ${is_solo} ]; then + echo "solo can't update artoo" + return 1 + fi + update_apply_artoo ${2} + ;; + "pixhawk" ) + if [ ! ${is_solo} ]; then + echo "controller can't update pixhawk" + return 1 + fi + update_apply_artoo ${2} + ;; + "sololink" ) + update_apply_sololink ${2} ${3} + ;; + * ) + echo "--update-apply [artoo|pixhawk|sololink] [--force] [--reset]" + return 1 + ;; + esac + return $? +} + + +# Get running image. +# Prints either "golden" or "latest". +get_image() { + if [ ! -z "${SOLOLINK_CONFIG_TEST}" ]; then + echo "no test mode for get_image" + return 1 + fi + boot_dev=`grep 'boot' /proc/mounts | awk '{print $1}'` + # boot_dev should be either: + # /dev/mmcblk0p1 when running golden, or + # /dev/mmcblk0p2 when running latest + if [ ${boot_dev} == "/dev/mmcblk0p1" ]; then + echo "golden" + return 0 + elif [ ${boot_dev} == "/dev/mmcblk0p2" ]; then + echo "latest" + return 0 + else + echo "can't determine boot image" + return 1 + fi +} + + +# Examples of valid MACs: +# 00:1f:09:04:00:22 (lower case hex) +# 00:1F:09:04:00:22 (upper case hex) +# 00-1F-09-04-00-22 (dash separators) +# 001f09040022 (no separators) +validate_mac() { + # convert hex to lower case, strip separators + mac=`echo "${1}" | tr A-Z a-z | tr -d :-` + maclen=${#mac} + if [ ${maclen} -ne 12 ]; then + echo "mac address invalid" + return 1 + fi + # put back the colons + mac="${mac:0:2}:${mac:2:2}:${mac:4:2}:${mac:6:2}:${mac:8:2}:${mac:10:2}" + echo "${mac}" + return 0 +} + + +# set_pairing_conf (MAC) [NAME] +# MAC required +# NAME is optional, defaults to "Solo" +# (controller only) +set_pairing_conf() { + mac=`validate_mac "${1}"` + if [ $? -ne 0 ]; then + return 1 + fi + if [ ! -z "${SOLOLINK_CONFIG_TEST}" ]; then + # might not exist in test directory structure + mkdir -p ${ROOT}/log + fi + echo "[${mac}]" > ${ROOT}/log/3dr-pairing.conf + if [ -z "${2}" ]; then + echo "name = Solo" >> ${ROOT}/log/3dr-pairing.conf + else + echo "name = ${2}" >> ${ROOT}/log/3dr-pairing.conf + fi + return 0 +} + + +get_pairing() { + if [ ${is_solo} ]; then + if wpa_supp_has_ssid; then + pardner=`get_wifi_ssid_wpa_supplicant` + else + echo "not paired" + return 1 + fi + else + if [ -f ${ROOT}/log/3dr-pairing.conf ]; then + pardner=`head -n 1 ${ROOT}/log/3dr-pairing.conf` + pardner=${pardner:1:17} + else + echo "not paired" + return 1 + fi + fi + echo ${pardner} + return 0 +} + + +set_pairing() { + if [ ${is_solo} ]; then + if [ -z "${1}" ]; then + cp ${ROOT}/mnt/rootfs.ro/etc/wpa_supplicant.orig ${ROOT}/etc/wpa_supplicant.conf + md5sum_config ${ROOT}/etc/wpa_supplicant + echo "not paired" + return 0 + else + set_wifi_ssid "${1}" + fi + else + if [ -z "${1}" ]; then + rm -f ${ROOT}/log/3dr-pairing.conf + echo "not paired" + return 0 + else + set_pairing_conf "${1}" + fi + fi + get_pairing +} + + +get_config() { + if grep -q "${1}" ${ROOT}/etc/sololink.conf; then + echo "`grep ${1} ${ROOT}/etc/sololink.conf | head -n 1 | \ + sed "s/[ \t]*${1}[ \t]*=[ \t]*\(.\+\)/\1/g"`" + return 0 + else + echo "${1} not set" + return 1 + fi +} + + +# set_config NAME VALUE [--add] +set_config() { + backup_config ${ROOT}/etc/sololink + if grep -q "${1}" ${ROOT}/etc/sololink.conf; then + # change existing setting + sed -i "s/[ \t]*${1}[ \t]*=.\+/${1}=${2}/g" ${ROOT}/etc/sololink.conf + elif [ "${3}_" == "--add_" ]; then + # append as new setting + echo >> ${ROOT}/etc/sololink.conf + echo "${1}=${2}" >> ${ROOT}/etc/sololink.conf + fi + md5sum_config ${ROOT}/etc/sololink + get_config "${1}" + return $? +} + + +# return success (0) if there is an invalid sticks message in 3dr-stm32.log +# after the most recent startup message +# +# Note that doing a stick cal will restart the stm32 in the current log; +# that is why we look for the error message _after_ the most recent startup +get_invalid_sticks() { + msg_start="main: starting" + msg_error="stm32: received invalid stick inputs message" + log=/log/3dr-stm32.log + # line number of most recent startup + line=`grep -n "${msg_start}" ${log} | tail -1 | sed 's/\([0-9]\+\).*/\1/g'` + # search file after that + if tail +${line} ${log} | grep -q "${msg_error}"; then + echo "`tail +${line} ${log} | grep -o \"${msg_error}\"`" + # found error, return success + return 0 + else + # did not find error, return failure + return 1 + fi +} + + +# get_status - get overall system status +# +# return status == 0 +# stdout == "ok" +# +# return status != 0 +# stdout == "error messages, one per line" +# +# The intent is that this can be augmented to look for problems other than +# "invalid sticks" if we need to. +get_status() { + status=0 + errmsg="ok" + if msg=`get_invalid_sticks`; then + status=1 + errmsg="${msg}" + fi + echo "${errmsg}" + return ${status} +} + + +get_version_golden() { + if [ ! -z "${SOLOLINK_CONFIG_TEST}" ]; then + echo "no test mode for get_version_golden" + return 1 + fi + image=`get_image` + case "${image}" in + "golden" ) + # running golden + version_file=${ROOT}/VERSION + ;; + "latest" ) + # running latest + mkdir -p ${ROOT}/mnt/golden + mount /dev/mmcblk0p1 ${ROOT}/mnt/golden + mkdir -p ${ROOT}/mnt/golden-squashfs + mount /mnt/golden/*.squashfs /mnt/golden-squashfs + version_file=${ROOT}/mnt/golden-squashfs/VERSION + ;; + * ) + echo "${image}" + return 1 + ;; + esac + + if [ ! -f ${version_file} ]; then + echo "unavailable" + return 1 + fi + + head -n 1 ${version_file} + + if [ "${image}" == "latest" ]; then + umount ${ROOT}/mnt/golden-squashfs + rmdir ${ROOT}/mnt/golden-squashfs + umount ${ROOT}/mnt/golden + rmdir ${ROOT}/mnt/golden + fi + + return 0 +} + + +# Get a version number from a file, or "unknown" if the file does not exist +# No trailing newline. +get_file_version() { + if [ -f "$1" ]; then + head -n 1 $1 | awk '{printf($1)}' + else + echo -n "unknown" + fi +} + + +# Get all versions; returns three lines: +# script_version +# loc_sololink_version loc_firmware_version +# rem_sololink_version rem_firmware_version +get_version_all() { + echo "${sololink_config_version}" + echo -n "`get_file_version ${ROOT}/VERSION` " + if [ ${is_controller} ]; then + echo `get_file_version ${ROOT}/STM_VERSION` + else + echo `get_file_version ${ROOT}/PIX_VERSION` + fi + echo -n "`get_file_version ${ROOT}/tmp/PEER_SL_VERSION` " + echo `get_file_version ${ROOT}/tmp/PEER_FW_VERSION` + return 0 +} + + +# get_version [artoo|pixhawk|sololink|golden|all] +get_version() { + if [ -z "${1}" ]; then + echo "${sololink_config_version}" + else + case "${1}" in + "artoo" ) + if [ ! ${is_controller} ]; then + echo "only controller can return artoo's version" + return 1 + fi + version_file=${ROOT}/STM_VERSION + ;; + "pixhawk" ) + if [ ! ${is_solo} ]; then + echo "only solo can return pixhawk's version" + return 1 + fi + version_file=${ROOT}/PIX_VERSION + ;; + "sololink" ) + version_file=${ROOT}/VERSION + ;; + "golden" ) + get_version_golden + return $? + ;; + "all" ) + get_version_all + return $? + ;; + * ) + echo "--get-version [artoo|pixhawk|sololink|golden|all]" + return 1 + ;; + esac + if [ ! -f ${version_file} ]; then + echo "unavailable" + return 1 + fi + head -n 1 ${version_file} + fi + return 0 +} + + +restart_sololink() { + init 2 + sleep 3 + init 3 + return 0 +} + + +do_restart() { + case "${1}" in + "video" ) + restart_video + return $? + ;; + "system" ) + shutdown_now + return 0 + ;; + "sololink" ) + restart_sololink + return $? + ;; + * ) + echo "--restart [video|sololink|system]" + return 1 + ;; + esac +} + + +# solo or controller? +if [ "_${HOSTNAME}" == "_3dr_solo" ]; then + is_solo=1 +elif [ "_${HOSTNAME}" == "_3dr_controller" ]; then + is_controller=1 +elif [ "_${TESTHOSTNAME}" == "_3dr_solo" ]; then + is_solo=1 +elif [ "_${TESTHOSTNAME}" == "_3dr_controller" ]; then + is_controller=1 +else + echo "hostname is not 3dr_solo or 3dr_controller; can't continue" + exit 1 +fi + +if [ -z "${1}" ]; then usage; fi + +case "${1}" in + "--get-wifi-ssid" ) + shift + get_wifi_ssid + ;; + "--set-wifi-ssid" ) + shift + if [ -z "${1}" ]; then usage; fi + set_wifi_ssid "${1}" + ;; + "--get-wifi-password" ) + shift + get_wifi_password + ;; + "--set-wifi-password" ) + shift + if [ -z "${1}" ]; then usage; fi + set_wifi_password "${1}" + ;; + "--get-wifi-channel" ) + shift + get_wifi_channel + ;; + "--set-wifi-channel" ) + shift + if [ -z "${1}" ]; then usage; fi + set_wifi_channel "${1}" + ;; + "--get-wifi-country" ) + shift + get_wifi_country + ;; + "--set-wifi-country" ) + shift + if [ -z "${1}" ]; then usage; fi + set_wifi_country "${1}" + ;; + "--get-wifi-power" ) + shift + get_wifi_power + ;; + "--set-wifi-power" ) + shift + if [ -z "${1}" ]; then usage; fi + set_wifi_power "${1}" + ;; + "--reset-wifi-settings" ) + shift + reset_wifi_settings + ;; + "--restart-video" ) + shift + restart_video + ;; + "--get-ui-mode" ) + shift + get_ui_mode + ;; + "--set-ui-mode" ) + shift + set_ui_mode "${1}" + ;; + "--get-ui-units" ) + shift + get_ui_units + ;; + "--set-ui-units" ) + shift + set_ui_units "${1}" + ;; + "--get-ui-sweep" ) + shift + get_ui_sweep + ;; + "--set-ui-sweep" ) + shift + set_ui_sweep "${1}" "${2}" + ;; + "--factory-reset" ) + factory_reset + ;; + "--settings-reset" ) + settings_reset + ;; + "--make-golden" ) + make_golden + ;; + "--update-prepare" ) + shift + if [ -z "${1}" ]; then usage; fi + update_prepare "${1}" + ;; + "--update-apply" ) + shift + if [ -z "${1}" ]; then usage; fi + update_apply "${1}" "${2}" "${3}" + ;; + "--get-image" ) + shift + get_image + ;; + "--get-pairing" ) + shift + get_pairing + ;; + "--set-pairing" ) + shift + # ${1} optional; empty means unpair + set_pairing "${1}" + ;; + "--get-config" ) + shift + if [ -z "${1}" ]; then usage; fi + get_config "${1}" + ;; + "--set-config" ) + shift + if [ -z "${1}" ] || [ -z "${2}" ]; then usage; fi + set_config $@ + ;; + "--get-status" ) + get_status + ;; + "--get-version" ) + shift + # ${1} optional; empty means get script version + get_version "${1}" + ;; + "--reboot" ) + shutdown_now + ;; + "--restart" ) + shift + if [ -z "${1}" ]; then usage; fi + do_restart "${1}" + ;; + "--pass" ) + shift + true + ;; + "--fail" ) + shift + false + ;; + * ) + usage +esac + +exit $? diff --git a/config/sololink_config_sample b/config/sololink_config_sample new file mode 100755 index 0000000..e1cb3da --- /dev/null +++ b/config/sololink_config_sample @@ -0,0 +1,28 @@ +#!/bin/bash + +# run_cmd prints a command, then the command's output, then 'ok' or 'error', +# depending on the command's exit status, then a blank line. +# +# All text after the command and before the 'ok' or 'error' is what the +# command sends to stdout. +run_cmd() { +echo $1 +$1 +if [ $? -eq 0 ]; then echo "ok"; else echo "error"; fi +echo +} + +for SSH in "ssh root@10.1.1.1" "ssh root@10.1.1.10"; do + +run_cmd "$SSH sololink_config --get-version" +run_cmd "$SSH sololink_config --get-version sololink" +run_cmd "$SSH sololink_config --get-version artoo" +run_cmd "$SSH sololink_config --get-version pixhawk" +run_cmd "$SSH sololink_config --get-version golden" +run_cmd "$SSH sololink_config --get-version all" +run_cmd "$SSH sololink_config --get-wifi-ssid" +run_cmd "$SSH sololink_config --get-wifi-password" +run_cmd "$SSH sololink_config --get-image" +run_cmd "$SSH sololink_config --get-pairing" + +done diff --git a/config/sololink_config_test b/config/sololink_config_test new file mode 100755 index 0000000..4ded026 --- /dev/null +++ b/config/sololink_config_test @@ -0,0 +1,689 @@ +#!/bin/bash + +# runs on ubuntu-14.04 +# does not run on osx + +if [ -z ${TESTHOSTNAME} ]; then + echo "ERROR: set TESTHOSTNAME" + echo " TESTHOSTNAME=3dr_solo ./sololink_config_test" + echo " TESTHOSTNAME=3dr_controller ./sololink_config_test" + exit 1 +fi + +export SOLOLINK_CONFIG_TEST=1 +export ROOT=./test_${TESTHOSTNAME} + +error=0 + +if ! ./sololink_config --pass; then + echo "ERROR at line ${LINENO}" + error=1 +fi + +if ./sololink_config --fail; then + echo "ERROR at line ${LINENO}" + error=1 +fi + +### WIFI SSID + +# backup, ignoring errors +cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1 +cp ${ROOT}/etc/hostapd.conf hostapd.save > /dev/null 2>&1 + +# get to initial state +./sololink_config --reset-wifi-settings + +echo -n "wifi: set ssid; not set ... " +x=`./sololink_config --set-wifi-ssid SoloLink_one` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "SoloLink_one" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set ssid; already set ... " +x=`./sololink_config --set-wifi-ssid SoloLink_two` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "SoloLink_two" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set ssid; too short ... " +x=`./sololink_config --set-wifi-ssid 1234567` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set ssid; not SoloLink_ ... " +x=`./sololink_config --set-wifi-ssid 12345678` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set ssid; max length ... " +xx="SoloLink_12345678901234567890123" +x=`./sololink_config --set-wifi-ssid $xx` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "$xx" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set ssid; too long ... " +x=`./sololink_config --set-wifi-ssid SoloLink_123456789012345678901234` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +# restore +if [ -f wpa_supplicant.save ]; then + mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf +fi +if [ -f hostapd.save ]; then + mv hostapd.save ${ROOT}/etc/hostapd.conf +fi + +### WIFI PASSWORD + +# backup, ignoring errors +cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1 +cp ${ROOT}/etc/hostapd.conf hostapd.save > /dev/null 2>&1 + +# get to initial state +./sololink_config --reset-wifi-settings + +echo -n "wifi: set password; not set ... " +x=`./sololink_config --set-wifi-password 12345678` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "12345678" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set password; already set ... " +x=`./sololink_config --set-wifi-password 87654321` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "87654321" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set password; with space ... " +x=`./sololink_config --set-wifi-password "ABCD EFGH"` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "ABCD EFGH" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set password; max length ... " +xx=123456789012345678901234567890123456789012345678901234567890123 +x=`./sololink_config --set-wifi-password $xx` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ "$x" != "$xx" ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set password; too short ... " +x=`./sololink_config --set-wifi-password 1234567` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "wifi: set password; too long ... " +xx=1234567890123456789012345678901234567890123456789012345678901234 +x=`./sololink_config --set-wifi-password $xx` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +# restore +if [ -f wpa_supplicant.save ]; then + mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf +fi +if [ -f hostapd.save ]; then + mv hostapd.save ${ROOT}/etc/hostapd.conf +fi + +### PAIR/UNPAIR + +# backup, ignoring errors +cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1 +cp ${ROOT}/log/3dr-pairing.conf 3dr-pairing.save > /dev/null 2>&1 + +echo -n "pair: set pairing; unpair ... " +x=`./sololink_config --set-pairing` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if grep -q "network=" ${ROOT}/etc/wpa_supplicant.conf; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi + else + if [ -f ${ROOT}/log/3dr-pairing.conf ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi + fi +fi + +echo -n "pair: get pairing; when unpaired ... " +pair=`./sololink_config --get-pairing` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "pair: set pairing; not paired ... " +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + peer="SoloLink_ABC" +elif [ "${TESTHOSTNAME}" == "3dr_controller" ]; then + peer="12:34:56:78:90:ab" +fi +pair=`./sololink_config --set-pairing ${peer}` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if ! grep -q "ssid=\"${peer}\"" ${ROOT}/etc/wpa_supplicant.conf; then + echo "ERROR at line ${LINENO}" + cat ${ROOT}/etc/wpa_supplicant.conf + error=1 + else + echo "OK" + fi + else + if [ ! -f ${ROOT}/log/3dr-pairing.conf ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi + fi +fi + +echo -n "pair: set pairing; already paired ... " +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + peer="SoloLink_123" +elif [ "${TESTHOSTNAME}" == "3dr_controller" ]; then + peer="cd:EF:ab:11:22:9A" +fi +x=`./sololink_config --set-pairing ${peer}` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if ! grep -q "ssid=\"${peer}\"" ${ROOT}/etc/wpa_supplicant.conf; then + echo "ERROR at line ${LINENO}" + cat ${ROOT}/etc/wpa_supplicant.conf + error=1 + else + echo "OK" + fi + else + if [ ! -f ${ROOT}/log/3dr-pairing.conf ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi + fi +fi + +# restore +if [ -f wpa_supplicant.save ]; then + mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf +fi +if [ -f 3dr-pairing.save ]; then + mv 3dr-pairing.save ${ROOT}/log/3dr-pairing.conf +else + rm -f ${ROOT}/log/3dr-pairing.conf +fi + +### UPDATE + +echo -n "update: factory reset ... " +x=`./sololink_config --factory-reset` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ ! -f ${ROOT}/log/updates/FACTORYRESET ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "update: settings reset ... " +x=`./sololink_config --settings-reset` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ ! -f ${ROOT}/log/updates/RESETSETTINGS ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +dir_empty() { + [ -d ${1} ] && [ `find ${1} | wc -l` -eq 1 ] +} + +echo -n "update: prepare artoo ... " +x=`./sololink_config --update-prepare artoo` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +else + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + elif ! dir_empty ${ROOT}/firmware; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +fi + +echo -n "update: apply artoo ... " +x=`./sololink_config --update-apply artoo` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +else + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +fi + +echo -n "update: prepare pixhawk ... " +x=`./sololink_config --update-prepare pixhawk` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + elif ! dir_empty ${ROOT}/firmware; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +else + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +fi + +echo -n "update: apply pixhawk ... " +x=`./sololink_config --update-apply pixhawk` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +else + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 + else + echo "OK" + fi +fi + +echo -n "update: prepare sololink ... " +x=`./sololink_config --update-prepare sololink` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif ! dir_empty ${ROOT}/log/updates; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +echo -n "update: apply sololink; correct file ... " +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + updatefile="3dr-solo-1234.tar.gz" +else + updatefile="3dr-controller-1234.tar.gz" +fi +touch ${ROOT}/log/updates/${updatefile} +pushd ${ROOT}/log/updates > /dev/null 2>&1 +md5sum ${updatefile} > ${updatefile}.md5 +popd > /dev/null 2>&1 +x=`./sololink_config --update-apply sololink` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +elif [ ! -f ${ROOT}/log/updates/UPDATE ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi +rm ${ROOT}/log/updates/* + +echo -n "update: apply sololink; incorrect file ... " +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + updatefile="3dr-controller-1234.tar.gz" +else + updatefile="3dr-solo-1234.tar.gz" +fi +touch ${ROOT}/log/updates/${updatefile} +pushd ${ROOT}/log/updates > /dev/null 2>&1 +md5sum ${updatefile} > ${updatefile}.md5 +popd > /dev/null 2>&1 +x=`./sololink_config --update-apply sololink` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi +rm ${ROOT}/log/updates/${updatefile}* + +echo -n "update: apply sololink; bad md5 ... " +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + updatefile="3dr-solo-1234.tar.gz" +else + updatefile="3dr-controller-1234.tar.gz" +fi +touch ${ROOT}/log/updates/${updatefile} +pushd ${ROOT}/log/updates > /dev/null 2>&1 +md5sum ${updatefile} > ${updatefile}.md5 +md5sum ${updatefile}.md5 > ${updatefile}.md5 +popd > /dev/null 2>&1 +x=`./sololink_config --update-apply sololink` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi +rm ${ROOT}/log/updates/${updatefile}* + +echo -n "update: apply sololink; missing file ... " +x=`./sololink_config --update-apply sololink` +if [ $? -eq 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "OK" +fi + +### VERSION + +echo -n "version: script ... " +x=`./sololink_config --get-version` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}" + error=1 +else + echo "${x} ... OK" +fi + +echo -n "version: sololink ... " +x=`./sololink_config --get-version sololink` +if [ $? -ne 0 ]; then + echo "ERROR at line ${LINENO}: ${x}" + error=1 +else + echo "${x} ... OK" +fi + +echo -n "version: artoo ... " +x=`./sololink_config --get-version artoo` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}: ${x}" + error=1 + else + echo "${x} ... OK" + fi +else + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}: ${x}" + error=1 + else + echo "${x} ... OK" + fi +fi + +echo -n "version: pixhawk ... " +x=`./sololink_config --get-version pixhawk` +r=$? +if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then + if [ ${r} -ne 0 ]; then + echo "ERROR at line ${LINENO}: ${x}" + error=1 + else + echo "${x} ... OK" + fi +else + if [ ${r} -eq 0 ]; then + echo "ERROR at line ${LINENO}: ${x}" + error=1 + else + echo "${x} ... OK" + fi +fi + +### GET/SET CONFIG + +# backup, ignoring errors +cp ${ROOT}/etc/sololink.conf sololink.save > /dev/null 2>&1 +cp ${ROOT}/etc/sololink.conf.md5 sololink.save.md5 > /dev/null 2>&1 + +# append test variables +cat >> ${ROOT}/etc/sololink.conf <' can be used to set it. +rts_threshold=2347 + +# Fragmentation threshold; 2346 = disabled (default); range 256..2346 +# If this field is not included in hostapd.conf, hostapd will not control +# fragmentation threshold and 'iwconfig wlan# frag ' can be used to set +# it. +fragm_threshold=2346 + +# Rate configuration +# Default is to enable all rates supported by the hardware. This configuration +# item allows this list be filtered so that only the listed rates will be left +# in the list. If the list is empty, all rates are used. This list can have +# entries that are not in the list of rates the hardware supports (such entries +# are ignored). The entries in this list are in 100 kbps, i.e., 11 Mbps = 110. +# If this item is present, at least one rate have to be matching with the rates +# hardware supports. +# default: use the most common supported rate setting for the selected +# hw_mode (i.e., this line can be removed from configuration file in most +# cases) +#supported_rates=10 20 55 110 60 90 120 180 240 360 480 540 + +# Basic rate set configuration +# List of rates (in 100 kbps) that are included in the basic rate set. +# If this item is not included, usually reasonable default set is used. +#basic_rates=10 20 +#basic_rates=10 20 55 110 +#basic_rates=60 120 240 + +# Short Preamble +# This parameter can be used to enable optional use of short preamble for +# frames sent at 2 Mbps, 5.5 Mbps, and 11 Mbps to improve network performance. +# This applies only to IEEE 802.11b-compatible networks and this should only be +# enabled if the local hardware supports use of short preamble. If any of the +# associated STAs do not support short preamble, use of short preamble will be +# disabled (and enabled when such STAs disassociate) dynamically. +# 0 = do not allow use of short preamble (default) +# 1 = allow use of short preamble +#preamble=1 + +# Station MAC address -based authentication +# Please note that this kind of access control requires a driver that uses +# hostapd to take care of management frame processing and as such, this can be +# used with driver=hostap or driver=nl80211, but not with driver=madwifi. +# 0 = accept unless in deny list +# 1 = deny unless in accept list +# 2 = use external RADIUS server (accept/deny lists are searched first) +macaddr_acl=0 + +# Accept/deny lists are read from separate files (containing list of +# MAC addresses, one per line). Use absolute path name to make sure that the +# files can be read on SIGHUP configuration reloads. +#accept_mac_file=/etc/hostapd.accept +#deny_mac_file=/etc/hostapd.deny + +# IEEE 802.11 specifies two authentication algorithms. hostapd can be +# configured to allow both of these or only one. Open system authentication +# should be used with IEEE 802.1X. +# Bit fields of allowed authentication algorithms: +# bit 0 = Open System Authentication +# bit 1 = Shared Key Authentication (requires WEP) +auth_algs=3 + +# Send empty SSID in beacons and ignore probe request frames that do not +# specify full SSID, i.e., require stations to know SSID. +# default: disabled (0) +# 1 = send empty (length=0) SSID in beacon and ignore probe request for +# broadcast SSID +# 2 = clear SSID (ASCII 0), but keep the original length (this may be required +# with some clients that do not support empty SSID) and ignore probe +# requests for broadcast SSID +ignore_broadcast_ssid=0 + +# Additional vendor specfic elements for Beacon and Probe Response frames +# This parameter can be used to add additional vendor specific element(s) into +# the end of the Beacon and Probe Response frames. The format for these +# element(s) is a hexdump of the raw information elements (id+len+payload for +# one or more elements) +#vendor_elements=dd0411223301 + +# TX queue parameters (EDCF / bursting) +# tx_queue__ +# queues: data0, data1, data2, data3, after_beacon, beacon +# (data0 is the highest priority queue) +# parameters: +# aifs: AIFS (default 2) +# cwmin: cwMin (1, 3, 7, 15, 31, 63, 127, 255, 511, 1023) +# cwmax: cwMax (1, 3, 7, 15, 31, 63, 127, 255, 511, 1023); cwMax >= cwMin +# burst: maximum length (in milliseconds with precision of up to 0.1 ms) for +# bursting +# +# Default WMM parameters (IEEE 802.11 draft; 11-03-0504-03-000e): +# These parameters are used by the access point when transmitting frames +# to the clients. +# +# Low priority / AC_BK = background +#tx_queue_data3_aifs=7 +#tx_queue_data3_cwmin=15 +#tx_queue_data3_cwmax=1023 +#tx_queue_data3_burst=0 +# Note: for IEEE 802.11b mode: cWmin=31 cWmax=1023 burst=0 +# +# Normal priority / AC_BE = best effort +#tx_queue_data2_aifs=3 +#tx_queue_data2_cwmin=15 +#tx_queue_data2_cwmax=63 +#tx_queue_data2_burst=0 +# Note: for IEEE 802.11b mode: cWmin=31 cWmax=127 burst=0 +# +# High priority / AC_VI = video +#tx_queue_data1_aifs=1 +#tx_queue_data1_cwmin=7 +#tx_queue_data1_cwmax=15 +#tx_queue_data1_burst=3.0 +# Note: for IEEE 802.11b mode: cWmin=15 cWmax=31 burst=6.0 +# +# Highest priority / AC_VO = voice +#tx_queue_data0_aifs=1 +#tx_queue_data0_cwmin=3 +#tx_queue_data0_cwmax=7 +#tx_queue_data0_burst=1.5 +# Note: for IEEE 802.11b mode: cWmin=7 cWmax=15 burst=3.3 + +# 802.1D Tag (= UP) to AC mappings +# WMM specifies following mapping of data frames to different ACs. This mapping +# can be configured using Linux QoS/tc and sch_pktpri.o module. +# 802.1D Tag 802.1D Designation Access Category WMM Designation +# 1 BK AC_BK Background +# 2 - AC_BK Background +# 0 BE AC_BE Best Effort +# 3 EE AC_BE Best Effort +# 4 CL AC_VI Video +# 5 VI AC_VI Video +# 6 VO AC_VO Voice +# 7 NC AC_VO Voice +# Data frames with no priority information: AC_BE +# Management frames: AC_VO +# PS-Poll frames: AC_BE + +# Default WMM parameters (IEEE 802.11 draft; 11-03-0504-03-000e): +# for 802.11a or 802.11g networks +# These parameters are sent to WMM clients when they associate. +# The parameters will be used by WMM clients for frames transmitted to the +# access point. +# +# note - txop_limit is in units of 32microseconds +# note - acm is admission control mandatory flag. 0 = admission control not +# required, 1 = mandatory +# note - here cwMin and cmMax are in exponent form. the actual cw value used +# will be (2^n)-1 where n is the value given here +# +wmm_enabled=1 +# +# WMM-PS Unscheduled Automatic Power Save Delivery [U-APSD] +# Enable this flag if U-APSD supported outside hostapd (eg., Firmware/driver) +#uapsd_advertisement_enabled=1 +# +# Low priority / AC_BK = background +wmm_ac_bk_cwmin=4 +wmm_ac_bk_cwmax=10 +wmm_ac_bk_aifs=7 +wmm_ac_bk_txop_limit=0 +wmm_ac_bk_acm=0 +# Note: for IEEE 802.11b mode: cWmin=5 cWmax=10 +# +# Normal priority / AC_BE = best effort +wmm_ac_be_aifs=3 +wmm_ac_be_cwmin=4 +wmm_ac_be_cwmax=10 +wmm_ac_be_txop_limit=0 +wmm_ac_be_acm=0 +# Note: for IEEE 802.11b mode: cWmin=5 cWmax=7 +# +# High priority / AC_VI = video +wmm_ac_vi_aifs=2 +wmm_ac_vi_cwmin=3 +wmm_ac_vi_cwmax=4 +wmm_ac_vi_txop_limit=94 +wmm_ac_vi_acm=0 +# Note: for IEEE 802.11b mode: cWmin=4 cWmax=5 txop_limit=188 +# +# Highest priority / AC_VO = voice +wmm_ac_vo_aifs=2 +wmm_ac_vo_cwmin=2 +wmm_ac_vo_cwmax=3 +wmm_ac_vo_txop_limit=47 +wmm_ac_vo_acm=0 +# Note: for IEEE 802.11b mode: cWmin=3 cWmax=4 burst=102 + +# Static WEP key configuration +# +# The key number to use when transmitting. +# It must be between 0 and 3, and the corresponding key must be set. +# default: not set +#wep_default_key=0 +# The WEP keys to use. +# A key may be a quoted string or unquoted hexadecimal digits. +# The key length should be 5, 13, or 16 characters, or 10, 26, or 32 +# digits, depending on whether 40-bit (64-bit), 104-bit (128-bit), or +# 128-bit (152-bit) WEP is used. +# Only the default key must be supplied; the others are optional. +# default: not set +#wep_key0=123456789a +#wep_key1="vwxyz" +#wep_key2=0102030405060708090a0b0c0d +#wep_key3=".2.4.6.8.0.23" + +# Station inactivity limit +# +# If a station does not send anything in ap_max_inactivity seconds, an +# empty data frame is sent to it in order to verify whether it is +# still in range. If this frame is not ACKed, the station will be +# disassociated and then deauthenticated. This feature is used to +# clear station table of old entries when the STAs move out of the +# range. +# +# The station can associate again with the AP if it is still in range; +# this inactivity poll is just used as a nicer way of verifying +# inactivity; i.e., client will not report broken connection because +# disassociation frame is not sent immediately without first polling +# the STA with a data frame. +# default: 300 (i.e., 5 minutes) +#ap_max_inactivity=300 +# +# The inactivity polling can be disabled to disconnect stations based on +# inactivity timeout so that idle stations are more likely to be disconnected +# even if they are still in range of the AP. This can be done by setting +# skip_inactivity_poll to 1 (default 0). +#skip_inactivity_poll=0 + +# Disassociate stations based on excessive transmission failures or other +# indications of connection loss. This depends on the driver capabilities and +# may not be available with all drivers. +#disassoc_low_ack=1 + +# Maximum allowed Listen Interval (how many Beacon periods STAs are allowed to +# remain asleep). Default: 65535 (no limit apart from field size) +#max_listen_interval=100 + +# WDS (4-address frame) mode with per-station virtual interfaces +# (only supported with driver=nl80211) +# This mode allows associated stations to use 4-address frames to allow layer 2 +# bridging to be used. +#wds_sta=1 + +# If bridge parameter is set, the WDS STA interface will be added to the same +# bridge by default. This can be overridden with the wds_bridge parameter to +# use a separate bridge. +#wds_bridge=wds-br0 + +# Start the AP with beaconing disabled by default. +#start_disabled=0 + +# Client isolation can be used to prevent low-level bridging of frames between +# associated stations in the BSS. By default, this bridging is allowed. +#ap_isolate=1 + +# Fixed BSS Load value for testing purposes +# This field can be used to configure hostapd to add a fixed BSS Load element +# into Beacon and Probe Response frames for testing purposes. The format is +# :: +#bss_load_test=12:80:20000 + +##### IEEE 802.11n related configuration ###################################### + +# ieee80211n: Whether IEEE 802.11n (HT) is enabled +# 0 = disabled (default) +# 1 = enabled +# Note: You will also need to enable WMM for full HT functionality. +ieee80211n=1 + +# ht_capab: HT capabilities (list of flags) +# LDPC coding capability: [LDPC] = supported +# Supported channel width set: [HT40-] = both 20 MHz and 40 MHz with secondary +# channel below the primary channel; [HT40+] = both 20 MHz and 40 MHz +# with secondary channel above the primary channel +# (20 MHz only if neither is set) +# Note: There are limits on which channels can be used with HT40- and +# HT40+. Following table shows the channels that may be available for +# HT40- and HT40+ use per IEEE 802.11n Annex J: +# freq HT40- HT40+ +# 2.4 GHz 5-13 1-7 (1-9 in Europe/Japan) +# 5 GHz 40,48,56,64 36,44,52,60 +# (depending on the location, not all of these channels may be available +# for use) +# Please note that 40 MHz channels may switch their primary and secondary +# channels if needed or creation of 40 MHz channel maybe rejected based +# on overlapping BSSes. These changes are done automatically when hostapd +# is setting up the 40 MHz channel. +# Spatial Multiplexing (SM) Power Save: [SMPS-STATIC] or [SMPS-DYNAMIC] +# (SMPS disabled if neither is set) +# HT-greenfield: [GF] (disabled if not set) +# Short GI for 20 MHz: [SHORT-GI-20] (disabled if not set) +# Short GI for 40 MHz: [SHORT-GI-40] (disabled if not set) +# Tx STBC: [TX-STBC] (disabled if not set) +# Rx STBC: [RX-STBC1] (one spatial stream), [RX-STBC12] (one or two spatial +# streams), or [RX-STBC123] (one, two, or three spatial streams); Rx STBC +# disabled if none of these set +# HT-delayed Block Ack: [DELAYED-BA] (disabled if not set) +# Maximum A-MSDU length: [MAX-AMSDU-7935] for 7935 octets (3839 octets if not +# set) +# DSSS/CCK Mode in 40 MHz: [DSSS_CCK-40] = allowed (not allowed if not set) +# 40 MHz intolerant [40-INTOLERANT] (not advertised if not set) +# L-SIG TXOP protection support: [LSIG-TXOP-PROT] (disabled if not set) +#ht_capab=[HT40-][SHORT-GI-20][SHORT-GI-40] + +# Require stations to support HT PHY (reject association if they do not) +#require_ht=1 + +# If set non-zero, require stations to perform scans of overlapping +# channels to test for stations which would be affected by 40 MHz traffic. +# This parameter sets the interval in seconds between these scans. This +# is useful only for testing that stations properly set the OBSS interval, +# since the other parameters in the OBSS scan parameters IE are set to 0. +#obss_interval=0 + +##### IEEE 802.11ac related configuration ##################################### + +# ieee80211ac: Whether IEEE 802.11ac (VHT) is enabled +# 0 = disabled (default) +# 1 = enabled +# Note: You will also need to enable WMM for full VHT functionality. +#ieee80211ac=1 + +# vht_capab: VHT capabilities (list of flags) +# +# vht_max_mpdu_len: [MAX-MPDU-7991] [MAX-MPDU-11454] +# Indicates maximum MPDU length +# 0 = 3895 octets (default) +# 1 = 7991 octets +# 2 = 11454 octets +# 3 = reserved +# +# supported_chan_width: [VHT160] [VHT160-80PLUS80] +# Indicates supported Channel widths +# 0 = 160 MHz & 80+80 channel widths are not supported (default) +# 1 = 160 MHz channel width is supported +# 2 = 160 MHz & 80+80 channel widths are supported +# 3 = reserved +# +# Rx LDPC coding capability: [RXLDPC] +# Indicates support for receiving LDPC coded pkts +# 0 = Not supported (default) +# 1 = Supported +# +# Short GI for 80 MHz: [SHORT-GI-80] +# Indicates short GI support for reception of packets transmitted with TXVECTOR +# params format equal to VHT and CBW = 80Mhz +# 0 = Not supported (default) +# 1 = Supported +# +# Short GI for 160 MHz: [SHORT-GI-160] +# Indicates short GI support for reception of packets transmitted with TXVECTOR +# params format equal to VHT and CBW = 160Mhz +# 0 = Not supported (default) +# 1 = Supported +# +# Tx STBC: [TX-STBC-2BY1] +# Indicates support for the transmission of at least 2x1 STBC +# 0 = Not supported (default) +# 1 = Supported +# +# Rx STBC: [RX-STBC-1] [RX-STBC-12] [RX-STBC-123] [RX-STBC-1234] +# Indicates support for the reception of PPDUs using STBC +# 0 = Not supported (default) +# 1 = support of one spatial stream +# 2 = support of one and two spatial streams +# 3 = support of one, two and three spatial streams +# 4 = support of one, two, three and four spatial streams +# 5,6,7 = reserved +# +# SU Beamformer Capable: [SU-BEAMFORMER] +# Indicates support for operation as a single user beamformer +# 0 = Not supported (default) +# 1 = Supported +# +# SU Beamformee Capable: [SU-BEAMFORMEE] +# Indicates support for operation as a single user beamformee +# 0 = Not supported (default) +# 1 = Supported +# +# Compressed Steering Number of Beamformer Antennas Supported: [BF-ANTENNA-2] +# Beamformee's capability indicating the maximum number of beamformer +# antennas the beamformee can support when sending compressed beamforming +# feedback +# If SU beamformer capable, set to maximum value minus 1 +# else reserved (default) +# +# Number of Sounding Dimensions: [SOUNDING-DIMENSION-2] +# Beamformer's capability indicating the maximum value of the NUM_STS parameter +# in the TXVECTOR of a VHT NDP +# If SU beamformer capable, set to maximum value minus 1 +# else reserved (default) +# +# MU Beamformer Capable: [MU-BEAMFORMER] +# Indicates support for operation as an MU beamformer +# 0 = Not supported or sent by Non-AP STA (default) +# 1 = Supported +# +# MU Beamformee Capable: [MU-BEAMFORMEE] +# Indicates support for operation as an MU beamformee +# 0 = Not supported or sent by AP (default) +# 1 = Supported +# +# VHT TXOP PS: [VHT-TXOP-PS] +# Indicates whether or not the AP supports VHT TXOP Power Save Mode +# or whether or not the STA is in VHT TXOP Power Save mode +# 0 = VHT AP doesnt support VHT TXOP PS mode (OR) VHT Sta not in VHT TXOP PS +# mode +# 1 = VHT AP supports VHT TXOP PS mode (OR) VHT Sta is in VHT TXOP power save +# mode +# +# +HTC-VHT Capable: [HTC-VHT] +# Indicates whether or not the STA supports receiving a VHT variant HT Control +# field. +# 0 = Not supported (default) +# 1 = supported +# +# Maximum A-MPDU Length Exponent: [MAX-A-MPDU-LEN-EXP0]..[MAX-A-MPDU-LEN-EXP7] +# Indicates the maximum length of A-MPDU pre-EOF padding that the STA can recv +# This field is an integer in the range of 0 to 7. +# The length defined by this field is equal to +# 2 pow(13 + Maximum A-MPDU Length Exponent) -1 octets +# +# VHT Link Adaptation Capable: [VHT-LINK-ADAPT2] [VHT-LINK-ADAPT3] +# Indicates whether or not the STA supports link adaptation using VHT variant +# HT Control field +# If +HTC-VHTcapable is 1 +# 0 = (no feedback) if the STA does not provide VHT MFB (default) +# 1 = reserved +# 2 = (Unsolicited) if the STA provides only unsolicited VHT MFB +# 3 = (Both) if the STA can provide VHT MFB in response to VHT MRQ and if the +# STA provides unsolicited VHT MFB +# Reserved if +HTC-VHTcapable is 0 +# +# Rx Antenna Pattern Consistency: [RX-ANTENNA-PATTERN] +# Indicates the possibility of Rx antenna pattern change +# 0 = Rx antenna pattern might change during the lifetime of an association +# 1 = Rx antenna pattern does not change during the lifetime of an association +# +# Tx Antenna Pattern Consistency: [TX-ANTENNA-PATTERN] +# Indicates the possibility of Tx antenna pattern change +# 0 = Tx antenna pattern might change during the lifetime of an association +# 1 = Tx antenna pattern does not change during the lifetime of an association +#vht_capab=[SHORT-GI-80][HTC-VHT] +# +# Require stations to support VHT PHY (reject association if they do not) +#require_vht=1 + +# 0 = 20 or 40 MHz operating Channel width +# 1 = 80 MHz channel width +# 2 = 160 MHz channel width +# 3 = 80+80 MHz channel width +#vht_oper_chwidth=1 +# +# center freq = 5 GHz + (5 * index) +# So index 42 gives center freq 5.210 GHz +# which is channel 42 in 5G band +# +#vht_oper_centr_freq_seg0_idx=42 +# +# center freq = 5 GHz + (5 * index) +# So index 159 gives center freq 5.795 GHz +# which is channel 159 in 5G band +# +#vht_oper_centr_freq_seg1_idx=159 + +##### IEEE 802.1X-2004 related configuration ################################## + +# Require IEEE 802.1X authorization +#ieee8021x=1 + +# IEEE 802.1X/EAPOL version +# hostapd is implemented based on IEEE Std 802.1X-2004 which defines EAPOL +# version 2. However, there are many client implementations that do not handle +# the new version number correctly (they seem to drop the frames completely). +# In order to make hostapd interoperate with these clients, the version number +# can be set to the older version (1) with this configuration value. +#eapol_version=2 + +# Optional displayable message sent with EAP Request-Identity. The first \0 +# in this string will be converted to ASCII-0 (nul). This can be used to +# separate network info (comma separated list of attribute=value pairs); see, +# e.g., RFC 4284. +#eap_message=hello +#eap_message=hello\0networkid=netw,nasid=foo,portid=0,NAIRealms=example.com + +# WEP rekeying (disabled if key lengths are not set or are set to 0) +# Key lengths for default/broadcast and individual/unicast keys: +# 5 = 40-bit WEP (also known as 64-bit WEP with 40 secret bits) +# 13 = 104-bit WEP (also known as 128-bit WEP with 104 secret bits) +#wep_key_len_broadcast=5 +#wep_key_len_unicast=5 +# Rekeying period in seconds. 0 = do not rekey (i.e., set keys only once) +#wep_rekey_period=300 + +# EAPOL-Key index workaround (set bit7) for WinXP Supplicant (needed only if +# only broadcast keys are used) +eapol_key_index_workaround=0 + +# EAP reauthentication period in seconds (default: 3600 seconds; 0 = disable +# reauthentication). +#eap_reauth_period=3600 + +# Use PAE group address (01:80:c2:00:00:03) instead of individual target +# address when sending EAPOL frames with driver=wired. This is the most common +# mechanism used in wired authentication, but it also requires that the port +# is only used by one station. +#use_pae_group_addr=1 + +##### Integrated EAP server ################################################### + +# Optionally, hostapd can be configured to use an integrated EAP server +# to process EAP authentication locally without need for an external RADIUS +# server. This functionality can be used both as a local authentication server +# for IEEE 802.1X/EAPOL and as a RADIUS server for other devices. + +# Use integrated EAP server instead of external RADIUS authentication +# server. This is also needed if hostapd is configured to act as a RADIUS +# authentication server. +eap_server=1 + +# Path for EAP server user database +# If SQLite support is included, this can be set to "sqlite:/path/to/sqlite.db" +# to use SQLite database instead of a text file. +#eap_user_file=/etc/hostapd.eap_user + +# CA certificate (PEM or DER file) for EAP-TLS/PEAP/TTLS +#ca_cert=/etc/hostapd.ca.pem + +# Server certificate (PEM or DER file) for EAP-TLS/PEAP/TTLS +#server_cert=/etc/hostapd.server.pem + +# Private key matching with the server certificate for EAP-TLS/PEAP/TTLS +# This may point to the same file as server_cert if both certificate and key +# are included in a single file. PKCS#12 (PFX) file (.p12/.pfx) can also be +# used by commenting out server_cert and specifying the PFX file as the +# private_key. +#private_key=/etc/hostapd.server.prv + +# Passphrase for private key +#private_key_passwd=secret passphrase + +# Server identity +# EAP methods that provide mechanism for authenticated server identity delivery +# use this value. If not set, "hostapd" is used as a default. +#server_id=server.example.com + +# Enable CRL verification. +# Note: hostapd does not yet support CRL downloading based on CDP. Thus, a +# valid CRL signed by the CA is required to be included in the ca_cert file. +# This can be done by using PEM format for CA certificate and CRL and +# concatenating these into one file. Whenever CRL changes, hostapd needs to be +# restarted to take the new CRL into use. +# 0 = do not verify CRLs (default) +# 1 = check the CRL of the user certificate +# 2 = check all CRLs in the certificate path +#check_crl=1 + +# Cached OCSP stapling response (DER encoded) +# If set, this file is sent as a certificate status response by the EAP server +# if the EAP peer requests certificate status in the ClientHello message. +# This cache file can be updated, e.g., by running following command +# periodically to get an update from the OCSP responder: +# openssl ocsp \ +# -no_nonce \ +# -CAfile /etc/hostapd.ca.pem \ +# -issuer /etc/hostapd.ca.pem \ +# -cert /etc/hostapd.server.pem \ +# -url http://ocsp.example.com:8888/ \ +# -respout /tmp/ocsp-cache.der +#ocsp_stapling_response=/tmp/ocsp-cache.der + +# dh_file: File path to DH/DSA parameters file (in PEM format) +# This is an optional configuration file for setting parameters for an +# ephemeral DH key exchange. In most cases, the default RSA authentication does +# not use this configuration. However, it is possible setup RSA to use +# ephemeral DH key exchange. In addition, ciphers with DSA keys always use +# ephemeral DH keys. This can be used to achieve forward secrecy. If the file +# is in DSA parameters format, it will be automatically converted into DH +# params. This parameter is required if anonymous EAP-FAST is used. +# You can generate DH parameters file with OpenSSL, e.g., +# "openssl dhparam -out /etc/hostapd.dh.pem 1024" +#dh_file=/etc/hostapd.dh.pem + +# Fragment size for EAP methods +#fragment_size=1400 + +# Finite cyclic group for EAP-pwd. Number maps to group of domain parameters +# using the IANA repository for IKE (RFC 2409). +#pwd_group=19 + +# Configuration data for EAP-SIM database/authentication gateway interface. +# This is a text string in implementation specific format. The example +# implementation in eap_sim_db.c uses this as the UNIX domain socket name for +# the HLR/AuC gateway (e.g., hlr_auc_gw). In this case, the path uses "unix:" +# prefix. If hostapd is built with SQLite support (CONFIG_SQLITE=y in .config), +# database file can be described with an optional db= parameter. +#eap_sim_db=unix:/tmp/hlr_auc_gw.sock +#eap_sim_db=unix:/tmp/hlr_auc_gw.sock db=/tmp/hostapd.db + +# Encryption key for EAP-FAST PAC-Opaque values. This key must be a secret, +# random value. It is configured as a 16-octet value in hex format. It can be +# generated, e.g., with the following command: +# od -tx1 -v -N16 /dev/random | colrm 1 8 | tr -d ' ' +#pac_opaque_encr_key=000102030405060708090a0b0c0d0e0f + +# EAP-FAST authority identity (A-ID) +# A-ID indicates the identity of the authority that issues PACs. The A-ID +# should be unique across all issuing servers. In theory, this is a variable +# length field, but due to some existing implementations requiring A-ID to be +# 16 octets in length, it is strongly recommended to use that length for the +# field to provid interoperability with deployed peer implementations. This +# field is configured in hex format. +#eap_fast_a_id=101112131415161718191a1b1c1d1e1f + +# EAP-FAST authority identifier information (A-ID-Info) +# This is a user-friendly name for the A-ID. For example, the enterprise name +# and server name in a human-readable format. This field is encoded as UTF-8. +#eap_fast_a_id_info=test server + +# Enable/disable different EAP-FAST provisioning modes: +#0 = provisioning disabled +#1 = only anonymous provisioning allowed +#2 = only authenticated provisioning allowed +#3 = both provisioning modes allowed (default) +#eap_fast_prov=3 + +# EAP-FAST PAC-Key lifetime in seconds (hard limit) +#pac_key_lifetime=604800 + +# EAP-FAST PAC-Key refresh time in seconds (soft limit on remaining hard +# limit). The server will generate a new PAC-Key when this number of seconds +# (or fewer) of the lifetime remains. +#pac_key_refresh_time=86400 + +# EAP-SIM and EAP-AKA protected success/failure indication using AT_RESULT_IND +# (default: 0 = disabled). +#eap_sim_aka_result_ind=1 + +# Trusted Network Connect (TNC) +# If enabled, TNC validation will be required before the peer is allowed to +# connect. Note: This is only used with EAP-TTLS and EAP-FAST. If any other +# EAP method is enabled, the peer will be allowed to connect without TNC. +#tnc=1 + + +##### IEEE 802.11f - Inter-Access Point Protocol (IAPP) ####################### + +# Interface to be used for IAPP broadcast packets +#iapp_interface=eth0 + + +##### RADIUS client configuration ############################################# +# for IEEE 802.1X with external Authentication Server, IEEE 802.11 +# authentication with external ACL for MAC addresses, and accounting + +# The own IP address of the access point (used as NAS-IP-Address) +own_ip_addr=127.0.0.1 + +# Optional NAS-Identifier string for RADIUS messages. When used, this should be +# a unique to the NAS within the scope of the RADIUS server. For example, a +# fully qualified domain name can be used here. +# When using IEEE 802.11r, nas_identifier must be set and must be between 1 and +# 48 octets long. +#nas_identifier=ap.example.com + +# RADIUS authentication server +#auth_server_addr=127.0.0.1 +#auth_server_port=1812 +#auth_server_shared_secret=secret + +# RADIUS accounting server +#acct_server_addr=127.0.0.1 +#acct_server_port=1813 +#acct_server_shared_secret=secret + +# Secondary RADIUS servers; to be used if primary one does not reply to +# RADIUS packets. These are optional and there can be more than one secondary +# server listed. +#auth_server_addr=127.0.0.2 +#auth_server_port=1812 +#auth_server_shared_secret=secret2 +# +#acct_server_addr=127.0.0.2 +#acct_server_port=1813 +#acct_server_shared_secret=secret2 + +# Retry interval for trying to return to the primary RADIUS server (in +# seconds). RADIUS client code will automatically try to use the next server +# when the current server is not replying to requests. If this interval is set, +# primary server will be retried after configured amount of time even if the +# currently used secondary server is still working. +#radius_retry_primary_interval=600 + + +# Interim accounting update interval +# If this is set (larger than 0) and acct_server is configured, hostapd will +# send interim accounting updates every N seconds. Note: if set, this overrides +# possible Acct-Interim-Interval attribute in Access-Accept message. Thus, this +# value should not be configured in hostapd.conf, if RADIUS server is used to +# control the interim interval. +# This value should not be less 600 (10 minutes) and must not be less than +# 60 (1 minute). +#radius_acct_interim_interval=600 + +# Request Chargeable-User-Identity (RFC 4372) +# This parameter can be used to configure hostapd to request CUI from the +# RADIUS server by including Chargeable-User-Identity attribute into +# Access-Request packets. +#radius_request_cui=1 + +# Dynamic VLAN mode; allow RADIUS authentication server to decide which VLAN +# is used for the stations. This information is parsed from following RADIUS +# attributes based on RFC 3580 and RFC 2868: Tunnel-Type (value 13 = VLAN), +# Tunnel-Medium-Type (value 6 = IEEE 802), Tunnel-Private-Group-ID (value +# VLANID as a string). Optionally, the local MAC ACL list (accept_mac_file) can +# be used to set static client MAC address to VLAN ID mapping. +# 0 = disabled (default) +# 1 = option; use default interface if RADIUS server does not include VLAN ID +# 2 = required; reject authentication if RADIUS server does not include VLAN ID +#dynamic_vlan=0 + +# VLAN interface list for dynamic VLAN mode is read from a separate text file. +# This list is used to map VLAN ID from the RADIUS server to a network +# interface. Each station is bound to one interface in the same way as with +# multiple BSSIDs or SSIDs. Each line in this text file is defining a new +# interface and the line must include VLAN ID and interface name separated by +# white space (space or tab). +# If no entries are provided by this file, the station is statically mapped +# to . interfaces. +#vlan_file=/etc/hostapd.vlan + +# Interface where 802.1q tagged packets should appear when a RADIUS server is +# used to determine which VLAN a station is on. hostapd creates a bridge for +# each VLAN. Then hostapd adds a VLAN interface (associated with the interface +# indicated by 'vlan_tagged_interface') and the appropriate wireless interface +# to the bridge. +#vlan_tagged_interface=eth0 + +# Bridge (prefix) to add the wifi and the tagged interface to. This gets the +# VLAN ID appended. It defaults to brvlan%d if no tagged interface is given +# and br%s.%d if a tagged interface is given, provided %s = tagged interface +# and %d = VLAN ID. +#vlan_bridge=brvlan + +# When hostapd creates a VLAN interface on vlan_tagged_interfaces, it needs +# to know how to name it. +# 0 = vlan, e.g., vlan1 +# 1 = ., e.g. eth0.1 +#vlan_naming=0 + +# Arbitrary RADIUS attributes can be added into Access-Request and +# Accounting-Request packets by specifying the contents of the attributes with +# the following configuration parameters. There can be multiple of these to +# add multiple attributes. These parameters can also be used to override some +# of the attributes added automatically by hostapd. +# Format: [:] +# attr_id: RADIUS attribute type (e.g., 26 = Vendor-Specific) +# syntax: s = string (UTF-8), d = integer, x = octet string +# value: attribute value in format indicated by the syntax +# If syntax and value parts are omitted, a null value (single 0x00 octet) is +# used. +# +# Additional Access-Request attributes +# radius_auth_req_attr=[:] +# Examples: +# Operator-Name = "Operator" +#radius_auth_req_attr=126:s:Operator +# Service-Type = Framed (2) +#radius_auth_req_attr=6:d:2 +# Connect-Info = "testing" (this overrides the automatically generated value) +#radius_auth_req_attr=77:s:testing +# Same Connect-Info value set as a hexdump +#radius_auth_req_attr=77:x:74657374696e67 + +# +# Additional Accounting-Request attributes +# radius_acct_req_attr=[:] +# Examples: +# Operator-Name = "Operator" +#radius_acct_req_attr=126:s:Operator + +# Dynamic Authorization Extensions (RFC 5176) +# This mechanism can be used to allow dynamic changes to user session based on +# commands from a RADIUS server (or some other disconnect client that has the +# needed session information). For example, Disconnect message can be used to +# request an associated station to be disconnected. +# +# This is disabled by default. Set radius_das_port to non-zero UDP port +# number to enable. +#radius_das_port=3799 +# +# DAS client (the host that can send Disconnect/CoA requests) and shared secret +#radius_das_client=192.168.1.123 shared secret here +# +# DAS Event-Timestamp time window in seconds +#radius_das_time_window=300 +# +# DAS require Event-Timestamp +#radius_das_require_event_timestamp=1 + +##### RADIUS authentication server configuration ############################## + +# hostapd can be used as a RADIUS authentication server for other hosts. This +# requires that the integrated EAP server is also enabled and both +# authentication services are sharing the same configuration. + +# File name of the RADIUS clients configuration for the RADIUS server. If this +# commented out, RADIUS server is disabled. +#radius_server_clients=/etc/hostapd.radius_clients + +# The UDP port number for the RADIUS authentication server +#radius_server_auth_port=1812 + +# The UDP port number for the RADIUS accounting server +# Commenting this out or setting this to 0 can be used to disable RADIUS +# accounting while still enabling RADIUS authentication. +#radius_server_acct_port=1813 + +# Use IPv6 with RADIUS server (IPv4 will also be supported using IPv6 API) +#radius_server_ipv6=1 + + +##### WPA/IEEE 802.11i configuration ########################################## + +# Enable WPA. Setting this variable configures the AP to require WPA (either +# WPA-PSK or WPA-RADIUS/EAP based on other configuration). For WPA-PSK, either +# wpa_psk or wpa_passphrase must be set and wpa_key_mgmt must include WPA-PSK. +# Instead of wpa_psk / wpa_passphrase, wpa_psk_radius might suffice. +# For WPA-RADIUS/EAP, ieee8021x must be set (but without dynamic WEP keys), +# RADIUS authentication server must be configured, and WPA-EAP must be included +# in wpa_key_mgmt. +# This field is a bit field that can be used to enable WPA (IEEE 802.11i/D3.0) +# and/or WPA2 (full IEEE 802.11i/RSN): +# bit0 = WPA +# bit1 = IEEE 802.11i/RSN (WPA2) (dot11RSNAEnabled) +wpa=2 + +# WPA pre-shared keys for WPA-PSK. This can be either entered as a 256-bit +# secret in hex format (64 hex digits), wpa_psk, or as an ASCII passphrase +# (8..63 characters) that will be converted to PSK. This conversion uses SSID +# so the PSK changes when ASCII passphrase is used and the SSID is changed. +# wpa_psk (dot11RSNAConfigPSKValue) +# wpa_passphrase (dot11RSNAConfigPSKPassPhrase) +#wpa_psk=0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef +wpa_passphrase=sololink + +# Optionally, WPA PSKs can be read from a separate text file (containing list +# of (PSK,MAC address) pairs. This allows more than one PSK to be configured. +# Use absolute path name to make sure that the files can be read on SIGHUP +# configuration reloads. +#wpa_psk_file=/etc/hostapd.wpa_psk + +# Optionally, WPA passphrase can be received from RADIUS authentication server +# This requires macaddr_acl to be set to 2 (RADIUS) +# 0 = disabled (default) +# 1 = optional; use default passphrase/psk if RADIUS server does not include +# Tunnel-Password +# 2 = required; reject authentication if RADIUS server does not include +# Tunnel-Password +#wpa_psk_radius=0 + +# Set of accepted key management algorithms (WPA-PSK, WPA-EAP, or both). The +# entries are separated with a space. WPA-PSK-SHA256 and WPA-EAP-SHA256 can be +# added to enable SHA256-based stronger algorithms. +# (dot11RSNAConfigAuthenticationSuitesTable) +#wpa_key_mgmt=WPA-PSK WPA-EAP + +# Set of accepted cipher suites (encryption algorithms) for pairwise keys +# (unicast packets). This is a space separated list of algorithms: +# CCMP = AES in Counter mode with CBC-MAC [RFC 3610, IEEE 802.11i/D7.0] +# TKIP = Temporal Key Integrity Protocol [IEEE 802.11i/D7.0] +# Group cipher suite (encryption algorithm for broadcast and multicast frames) +# is automatically selected based on this configuration. If only CCMP is +# allowed as the pairwise cipher, group cipher will also be CCMP. Otherwise, +# TKIP will be used as the group cipher. +# (dot11RSNAConfigPairwiseCiphersTable) +# Pairwise cipher for WPA (v1) (default: TKIP) +wpa_pairwise=TKIP CCMP +# Pairwise cipher for RSN/WPA2 (default: use wpa_pairwise value) +#rsn_pairwise=CCMP + +# Time interval for rekeying GTK (broadcast/multicast encryption keys) in +# seconds. (dot11RSNAConfigGroupRekeyTime) +#wpa_group_rekey=600 + +# Rekey GTK when any STA that possesses the current GTK is leaving the BSS. +# (dot11RSNAConfigGroupRekeyStrict) +#wpa_strict_rekey=1 + +# Time interval for rekeying GMK (master key used internally to generate GTKs +# (in seconds). +#wpa_gmk_rekey=86400 + +# Maximum lifetime for PTK in seconds. This can be used to enforce rekeying of +# PTK to mitigate some attacks against TKIP deficiencies. +#wpa_ptk_rekey=600 + +# Enable IEEE 802.11i/RSN/WPA2 pre-authentication. This is used to speed up +# roaming be pre-authenticating IEEE 802.1X/EAP part of the full RSN +# authentication and key handshake before actually associating with a new AP. +# (dot11RSNAPreauthenticationEnabled) +#rsn_preauth=1 +# +# Space separated list of interfaces from which pre-authentication frames are +# accepted (e.g., 'eth0' or 'eth0 wlan0wds0'. This list should include all +# interface that are used for connections to other APs. This could include +# wired interfaces and WDS links. The normal wireless data interface towards +# associated stations (e.g., wlan0) should not be added, since +# pre-authentication is only used with APs other than the currently associated +# one. +#rsn_preauth_interfaces=eth0 + +# peerkey: Whether PeerKey negotiation for direct links (IEEE 802.11e) is +# allowed. This is only used with RSN/WPA2. +# 0 = disabled (default) +# 1 = enabled +#peerkey=1 + +# ieee80211w: Whether management frame protection (MFP) is enabled +# 0 = disabled (default) +# 1 = optional +# 2 = required +#ieee80211w=0 + +# Group management cipher suite +# Default: AES-128-CMAC (BIP) +# Other options (depending on driver support): +# BIP-GMAC-128 +# BIP-GMAC-256 +# BIP-CMAC-256 +# Note: All the stations connecting to the BSS will also need to support the +# selected cipher. The default AES-128-CMAC is the only option that is commonly +# available in deployed devices. +#group_mgmt_cipher=AES-128-CMAC + +# Association SA Query maximum timeout (in TU = 1.024 ms; for MFP) +# (maximum time to wait for a SA Query response) +# dot11AssociationSAQueryMaximumTimeout, 1...4294967295 +#assoc_sa_query_max_timeout=1000 + +# Association SA Query retry timeout (in TU = 1.024 ms; for MFP) +# (time between two subsequent SA Query requests) +# dot11AssociationSAQueryRetryTimeout, 1...4294967295 +#assoc_sa_query_retry_timeout=201 + +# disable_pmksa_caching: Disable PMKSA caching +# This parameter can be used to disable caching of PMKSA created through EAP +# authentication. RSN preauthentication may still end up using PMKSA caching if +# it is enabled (rsn_preauth=1). +# 0 = PMKSA caching enabled (default) +# 1 = PMKSA caching disabled +#disable_pmksa_caching=0 + +# okc: Opportunistic Key Caching (aka Proactive Key Caching) +# Allow PMK cache to be shared opportunistically among configured interfaces +# and BSSes (i.e., all configurations within a single hostapd process). +# 0 = disabled (default) +# 1 = enabled +#okc=1 + +# SAE threshold for anti-clogging mechanism (dot11RSNASAEAntiCloggingThreshold) +# This parameter defines how many open SAE instances can be in progress at the +# same time before the anti-clogging mechanism is taken into use. +#sae_anti_clogging_threshold=5 + +# Enabled SAE finite cyclic groups +# SAE implementation are required to support group 19 (ECC group defined over a +# 256-bit prime order field). All groups that are supported by the +# implementation are enabled by default. This configuration parameter can be +# used to specify a limited set of allowed groups. The group values are listed +# in the IANA registry: +# http://www.iana.org/assignments/ipsec-registry/ipsec-registry.xml#ipsec-registry-9 +#sae_groups=19 20 21 25 26 + +##### IEEE 802.11r configuration ############################################## + +# Mobility Domain identifier (dot11FTMobilityDomainID, MDID) +# MDID is used to indicate a group of APs (within an ESS, i.e., sharing the +# same SSID) between which a STA can use Fast BSS Transition. +# 2-octet identifier as a hex string. +#mobility_domain=a1b2 + +# PMK-R0 Key Holder identifier (dot11FTR0KeyHolderID) +# 1 to 48 octet identifier. +# This is configured with nas_identifier (see RADIUS client section above). + +# Default lifetime of the PMK-RO in minutes; range 1..65535 +# (dot11FTR0KeyLifetime) +#r0_key_lifetime=10000 + +# PMK-R1 Key Holder identifier (dot11FTR1KeyHolderID) +# 6-octet identifier as a hex string. +#r1_key_holder=000102030405 + +# Reassociation deadline in time units (TUs / 1.024 ms; range 1000..65535) +# (dot11FTReassociationDeadline) +#reassociation_deadline=1000 + +# List of R0KHs in the same Mobility Domain +# format: <128-bit key as hex string> +# This list is used to map R0KH-ID (NAS Identifier) to a destination MAC +# address when requesting PMK-R1 key from the R0KH that the STA used during the +# Initial Mobility Domain Association. +#r0kh=02:01:02:03:04:05 r0kh-1.example.com 000102030405060708090a0b0c0d0e0f +#r0kh=02:01:02:03:04:06 r0kh-2.example.com 00112233445566778899aabbccddeeff +# And so on.. One line per R0KH. + +# List of R1KHs in the same Mobility Domain +# format: <128-bit key as hex string> +# This list is used to map R1KH-ID to a destination MAC address when sending +# PMK-R1 key from the R0KH. This is also the list of authorized R1KHs in the MD +# that can request PMK-R1 keys. +#r1kh=02:01:02:03:04:05 02:11:22:33:44:55 000102030405060708090a0b0c0d0e0f +#r1kh=02:01:02:03:04:06 02:11:22:33:44:66 00112233445566778899aabbccddeeff +# And so on.. One line per R1KH. + +# Whether PMK-R1 push is enabled at R0KH +# 0 = do not push PMK-R1 to all configured R1KHs (default) +# 1 = push PMK-R1 to all configured R1KHs whenever a new PMK-R0 is derived +#pmk_r1_push=1 + +##### Neighbor table ########################################################## +# Maximum number of entries kept in AP table (either for neigbor table or for +# detecting Overlapping Legacy BSS Condition). The oldest entry will be +# removed when adding a new entry that would make the list grow over this +# limit. Note! WFA certification for IEEE 802.11g requires that OLBC is +# enabled, so this field should not be set to 0 when using IEEE 802.11g. +# default: 255 +#ap_table_max_size=255 + +# Number of seconds of no frames received after which entries may be deleted +# from the AP table. Since passive scanning is not usually performed frequently +# this should not be set to very small value. In addition, there is no +# guarantee that every scan cycle will receive beacon frames from the +# neighboring APs. +# default: 60 +#ap_table_expiration_time=3600 + + +##### Wi-Fi Protected Setup (WPS) ############################################# + +# WPS state +# 0 = WPS disabled (default) +# 1 = WPS enabled, not configured +# 2 = WPS enabled, configured +wps_state=2 + +# Whether to manage this interface independently from other WPS interfaces +# By default, a single hostapd process applies WPS operations to all configured +# interfaces. This parameter can be used to disable that behavior for a subset +# of interfaces. If this is set to non-zero for an interface, WPS commands +# issued on that interface do not apply to other interfaces and WPS operations +# performed on other interfaces do not affect this interface. +#wps_independent=0 + +# AP can be configured into a locked state where new WPS Registrar are not +# accepted, but previously authorized Registrars (including the internal one) +# can continue to add new Enrollees. +#ap_setup_locked=1 + +# Universally Unique IDentifier (UUID; see RFC 4122) of the device +# This value is used as the UUID for the internal WPS Registrar. If the AP +# is also using UPnP, this value should be set to the device's UPnP UUID. +# If not configured, UUID will be generated based on the local MAC address. +#uuid=12345678-9abc-def0-1234-56789abcdef0 + +# Note: If wpa_psk_file is set, WPS is used to generate random, per-device PSKs +# that will be appended to the wpa_psk_file. If wpa_psk_file is not set, the +# default PSK (wpa_psk/wpa_passphrase) will be delivered to Enrollees. Use of +# per-device PSKs is recommended as the more secure option (i.e., make sure to +# set wpa_psk_file when using WPS with WPA-PSK). + +# When an Enrollee requests access to the network with PIN method, the Enrollee +# PIN will need to be entered for the Registrar. PIN request notifications are +# sent to hostapd ctrl_iface monitor. In addition, they can be written to a +# text file that could be used, e.g., to populate the AP administration UI with +# pending PIN requests. If the following variable is set, the PIN requests will +# be written to the configured file. +#wps_pin_requests=/var/run/hostapd_wps_pin_requests + +# Device Name +# User-friendly description of device; up to 32 octets encoded in UTF-8 +#device_name=Wireless AP + +# Manufacturer +# The manufacturer of the device (up to 64 ASCII characters) +#manufacturer=Company + +# Model Name +# Model of the device (up to 32 ASCII characters) +#model_name=WAP + +# Model Number +# Additional device description (up to 32 ASCII characters) +#model_number=123 + +# Serial Number +# Serial number of the device (up to 32 characters) +#serial_number=12345 + +# Primary Device Type +# Used format: -- +# categ = Category as an integer value +# OUI = OUI and type octet as a 4-octet hex-encoded value; 0050F204 for +# default WPS OUI +# subcateg = OUI-specific Sub Category as an integer value +# Examples: +# 1-0050F204-1 (Computer / PC) +# 1-0050F204-2 (Computer / Server) +# 5-0050F204-1 (Storage / NAS) +# 6-0050F204-1 (Network Infrastructure / AP) +#device_type=6-0050F204-1 + +# OS Version +# 4-octet operating system version number (hex string) +#os_version=01020300 + +# Config Methods +# List of the supported configuration methods +# Available methods: usba ethernet label display ext_nfc_token int_nfc_token +# nfc_interface push_button keypad virtual_display physical_display +# virtual_push_button physical_push_button +#config_methods=label virtual_display virtual_push_button keypad + +# WPS capability discovery workaround for PBC with Windows 7 +# Windows 7 uses incorrect way of figuring out AP's WPS capabilities by acting +# as a Registrar and using M1 from the AP. The config methods attribute in that +# message is supposed to indicate only the configuration method supported by +# the AP in Enrollee role, i.e., to add an external Registrar. For that case, +# PBC shall not be used and as such, the PushButton config method is removed +# from M1 by default. If pbc_in_m1=1 is included in the configuration file, +# the PushButton config method is left in M1 (if included in config_methods +# parameter) to allow Windows 7 to use PBC instead of PIN (e.g., from a label +# in the AP). +#pbc_in_m1=1 + +# Static access point PIN for initial configuration and adding Registrars +# If not set, hostapd will not allow external WPS Registrars to control the +# access point. The AP PIN can also be set at runtime with hostapd_cli +# wps_ap_pin command. Use of temporary (enabled by user action) and random +# AP PIN is much more secure than configuring a static AP PIN here. As such, +# use of the ap_pin parameter is not recommended if the AP device has means for +# displaying a random PIN. +#ap_pin=12345670 + +# Skip building of automatic WPS credential +# This can be used to allow the automatically generated Credential attribute to +# be replaced with pre-configured Credential(s). +#skip_cred_build=1 + +# Additional Credential attribute(s) +# This option can be used to add pre-configured Credential attributes into M8 +# message when acting as a Registrar. If skip_cred_build=1, this data will also +# be able to override the Credential attribute that would have otherwise been +# automatically generated based on network configuration. This configuration +# option points to an external file that much contain the WPS Credential +# attribute(s) as binary data. +#extra_cred=hostapd.cred + +# Credential processing +# 0 = process received credentials internally (default) +# 1 = do not process received credentials; just pass them over ctrl_iface to +# external program(s) +# 2 = process received credentials internally and pass them over ctrl_iface +# to external program(s) +# Note: With wps_cred_processing=1, skip_cred_build should be set to 1 and +# extra_cred be used to provide the Credential data for Enrollees. +# +# wps_cred_processing=1 will disabled automatic updates of hostapd.conf file +# both for Credential processing and for marking AP Setup Locked based on +# validation failures of AP PIN. An external program is responsible on updating +# the configuration appropriately in this case. +#wps_cred_processing=0 + +# AP Settings Attributes for M7 +# By default, hostapd generates the AP Settings Attributes for M7 based on the +# current configuration. It is possible to override this by providing a file +# with pre-configured attributes. This is similar to extra_cred file format, +# but the AP Settings attributes are not encapsulated in a Credential +# attribute. +#ap_settings=hostapd.ap_settings + +# WPS UPnP interface +# If set, support for external Registrars is enabled. +#upnp_iface=br0 + +# Friendly Name (required for UPnP) +# Short description for end use. Should be less than 64 characters. +#friendly_name=WPS Access Point + +# Manufacturer URL (optional for UPnP) +#manufacturer_url=http://www.example.com/ + +# Model Description (recommended for UPnP) +# Long description for end user. Should be less than 128 characters. +#model_description=Wireless Access Point + +# Model URL (optional for UPnP) +#model_url=http://www.example.com/model/ + +# Universal Product Code (optional for UPnP) +# 12-digit, all-numeric code that identifies the consumer package. +#upc=123456789012 + +# WPS RF Bands (a = 5G, b = 2.4G, g = 2.4G, ag = dual band) +# This value should be set according to RF band(s) supported by the AP if +# hw_mode is not set. For dual band dual concurrent devices, this needs to be +# set to ag to allow both RF bands to be advertized. +#wps_rf_bands=ag + +# NFC password token for WPS +# These parameters can be used to configure a fixed NFC password token for the +# AP. This can be generated, e.g., with nfc_pw_token from wpa_supplicant. When +# these parameters are used, the AP is assumed to be deployed with a NFC tag +# that includes the matching NFC password token (e.g., written based on the +# NDEF record from nfc_pw_token). +# +#wps_nfc_dev_pw_id: Device Password ID (16..65535) +#wps_nfc_dh_pubkey: Hexdump of DH Public Key +#wps_nfc_dh_privkey: Hexdump of DH Private Key +#wps_nfc_dev_pw: Hexdump of Device Password + +##### Wi-Fi Direct (P2P) ###################################################### + +# Enable P2P Device management +#manage_p2p=1 + +# Allow cross connection +#allow_cross_connection=1 + +#### TDLS (IEEE 802.11z-2010) ################################################# + +# Prohibit use of TDLS in this BSS +#tdls_prohibit=1 + +# Prohibit use of TDLS Channel Switching in this BSS +#tdls_prohibit_chan_switch=1 + +##### IEEE 802.11v-2011 ####################################################### + +# Time advertisement +# 0 = disabled (default) +# 2 = UTC time at which the TSF timer is 0 +#time_advertisement=2 + +# Local time zone as specified in 8.3 of IEEE Std 1003.1-2004: +# stdoffset[dst[offset][,start[/time],end[/time]]] +#time_zone=EST5 + +# WNM-Sleep Mode (extended sleep mode for stations) +# 0 = disabled (default) +# 1 = enabled (allow stations to use WNM-Sleep Mode) +#wnm_sleep_mode=1 + +# BSS Transition Management +# 0 = disabled (default) +# 1 = enabled +#bss_transition=1 + +##### IEEE 802.11u-2011 ####################################################### + +# Enable Interworking service +#interworking=1 + +# Access Network Type +# 0 = Private network +# 1 = Private network with guest access +# 2 = Chargeable public network +# 3 = Free public network +# 4 = Personal device network +# 5 = Emergency services only network +# 14 = Test or experimental +# 15 = Wildcard +#access_network_type=0 + +# Whether the network provides connectivity to the Internet +# 0 = Unspecified +# 1 = Network provides connectivity to the Internet +#internet=1 + +# Additional Step Required for Access +# Note: This is only used with open network, i.e., ASRA shall ne set to 0 if +# RSN is used. +#asra=0 + +# Emergency services reachable +#esr=0 + +# Unauthenticated emergency service accessible +#uesa=0 + +# Venue Info (optional) +# The available values are defined in IEEE Std 802.11u-2011, 7.3.1.34. +# Example values (group,type): +# 0,0 = Unspecified +# 1,7 = Convention Center +# 1,13 = Coffee Shop +# 2,0 = Unspecified Business +# 7,1 Private Residence +#venue_group=7 +#venue_type=1 + +# Homogeneous ESS identifier (optional; dot11HESSID) +# If set, this shall be identifical to one of the BSSIDs in the homogeneous +# ESS and this shall be set to the same value across all BSSs in homogeneous +# ESS. +#hessid=02:03:04:05:06:07 + +# Roaming Consortium List +# Arbitrary number of Roaming Consortium OIs can be configured with each line +# adding a new OI to the list. The first three entries are available through +# Beacon and Probe Response frames. Any additional entry will be available only +# through ANQP queries. Each OI is between 3 and 15 octets and is configured as +# a hexstring. +#roaming_consortium=021122 +#roaming_consortium=2233445566 + +# Venue Name information +# This parameter can be used to configure one or more Venue Name Duples for +# Venue Name ANQP information. Each entry has a two or three character language +# code (ISO-639) separated by colon from the venue name string. +# Note that venue_group and venue_type have to be set for Venue Name +# information to be complete. +#venue_name=eng:Example venue +#venue_name=fin:Esimerkkipaikka +# Alternative format for language:value strings: +# (double quoted string, printf-escaped string) +#venue_name=P"eng:Example\nvenue" + +# Network Authentication Type +# This parameter indicates what type of network authentication is used in the +# network. +# format: [redirect URL] +# Network Authentication Type Indicator values: +# 00 = Acceptance of terms and conditions +# 01 = On-line enrollment supported +# 02 = http/https redirection +# 03 = DNS redirection +#network_auth_type=00 +#network_auth_type=02http://www.example.com/redirect/me/here/ + +# IP Address Type Availability +# format: <1-octet encoded value as hex str> +# (ipv4_type & 0x3f) << 2 | (ipv6_type & 0x3) +# ipv4_type: +# 0 = Address type not available +# 1 = Public IPv4 address available +# 2 = Port-restricted IPv4 address available +# 3 = Single NATed private IPv4 address available +# 4 = Double NATed private IPv4 address available +# 5 = Port-restricted IPv4 address and single NATed IPv4 address available +# 6 = Port-restricted IPv4 address and double NATed IPv4 address available +# 7 = Availability of the address type is not known +# ipv6_type: +# 0 = Address type not available +# 1 = Address type available +# 2 = Availability of the address type not known +#ipaddr_type_availability=14 + +# Domain Name +# format: [,] +#domain_name=example.com,another.example.com,yet-another.example.com + +# 3GPP Cellular Network information +# format: [;][;...] +#anqp_3gpp_cell_net=244,91;310,026;234,56 + +# NAI Realm information +# One or more realm can be advertised. Each nai_realm line adds a new realm to +# the set. These parameters provide information for stations using Interworking +# network selection to allow automatic connection to a network based on +# credentials. +# format: ,[,][,][,...] +# encoding: +# 0 = Realm formatted in accordance with IETF RFC 4282 +# 1 = UTF-8 formatted character string that is not formatted in +# accordance with IETF RFC 4282 +# NAI Realm(s): Semi-colon delimited NAI Realm(s) +# EAP Method: [:<[AuthParam1:Val1]>][<[AuthParam2:Val2]>][...] +# EAP Method types, see: +# http://www.iana.org/assignments/eap-numbers/eap-numbers.xhtml#eap-numbers-4 +# AuthParam (Table 8-188 in IEEE Std 802.11-2012): +# ID 2 = Non-EAP Inner Authentication Type +# 1 = PAP, 2 = CHAP, 3 = MSCHAP, 4 = MSCHAPV2 +# ID 3 = Inner authentication EAP Method Type +# ID 5 = Credential Type +# 1 = SIM, 2 = USIM, 3 = NFC Secure Element, 4 = Hardware Token, +# 5 = Softoken, 6 = Certificate, 7 = username/password, 9 = Anonymous, +# 10 = Vendor Specific +#nai_realm=0,example.com;example.net +# EAP methods EAP-TLS with certificate and EAP-TTLS/MSCHAPv2 with +# username/password +#nai_realm=0,example.org,13[5:6],21[2:4][5:7] + +# QoS Map Set configuration +# +# Comma delimited QoS Map Set in decimal values +# (see IEEE Std 802.11-2012, 8.4.2.97) +# +# format: +# [,],... +# +# There can be up to 21 optional DSCP Exceptions which are pairs of DSCP Value +# (0..63 or 255) and User Priority (0..7). This is followed by eight DSCP Range +# descriptions with DSCP Low Value and DSCP High Value pairs (0..63 or 255) for +# each UP starting from 0. If both low and high value are set to 255, the +# corresponding UP is not used. +# +# default: not set +#qos_map_set=53,2,22,6,8,15,0,7,255,255,16,31,32,39,255,255,40,47,255,255 + +##### Hotspot 2.0 ############################################################# + +# Enable Hotspot 2.0 support +#hs20=1 + +# Disable Downstream Group-Addressed Forwarding (DGAF) +# This can be used to configure a network where no group-addressed frames are +# allowed. The AP will not forward any group-address frames to the stations and +# random GTKs are issued for each station to prevent associated stations from +# forging such frames to other stations in the BSS. +#disable_dgaf=1 + +# OSU Server-Only Authenticated L2 Encryption Network +#osen=1 + +# ANQP Domain ID (0..65535) +# An identifier for a set of APs in an ESS that share the same common ANQP +# information. 0 = Some of the ANQP information is unique to this AP (default). +#anqp_domain_id=1234 + +# Deauthentication request timeout +# If the RADIUS server indicates that the station is not allowed to connect to +# the BSS/ESS, the AP can allow the station some time to download a +# notification page (URL included in the message). This parameter sets that +# timeout in seconds. +#hs20_deauth_req_timeout=60 + +# Operator Friendly Name +# This parameter can be used to configure one or more Operator Friendly Name +# Duples. Each entry has a two or three character language code (ISO-639) +# separated by colon from the operator friendly name string. +#hs20_oper_friendly_name=eng:Example operator +#hs20_oper_friendly_name=fin:Esimerkkioperaattori + +# Connection Capability +# This can be used to advertise what type of IP traffic can be sent through the +# hotspot (e.g., due to firewall allowing/blocking protocols/ports). +# format: :: +# IP Protocol: 1 = ICMP, 6 = TCP, 17 = UDP +# Port Number: 0..65535 +# Status: 0 = Closed, 1 = Open, 2 = Unknown +# Each hs20_conn_capab line is added to the list of advertised tuples. +#hs20_conn_capab=1:0:2 +#hs20_conn_capab=6:22:1 +#hs20_conn_capab=17:5060:0 + +# WAN Metrics +# format: :
:
    :
    :
      : +# WAN Info: B0-B1: Link Status, B2: Symmetric Link, B3: At Capabity +# (encoded as two hex digits) +# Link Status: 1 = Link up, 2 = Link down, 3 = Link in test state +# Downlink Speed: Estimate of WAN backhaul link current downlink speed in kbps; +# 1..4294967295; 0 = unknown +# Uplink Speed: Estimate of WAN backhaul link current uplink speed in kbps +# 1..4294967295; 0 = unknown +# Downlink Load: Current load of downlink WAN connection (scaled to 255 = 100%) +# Uplink Load: Current load of uplink WAN connection (scaled to 255 = 100%) +# Load Measurement Duration: Duration for measuring downlink/uplink load in +# tenths of a second (1..65535); 0 if load cannot be determined +#hs20_wan_metrics=01:8000:1000:80:240:3000 + +# Operating Class Indication +# List of operating classes the BSSes in this ESS use. The Global operating +# classes in Table E-4 of IEEE Std 802.11-2012 Annex E define the values that +# can be used in this. +# format: hexdump of operating class octets +# for example, operating classes 81 (2.4 GHz channels 1-13) and 115 (5 GHz +# channels 36-48): +#hs20_operating_class=5173 + +# OSU icons +# ::::: +#hs20_icon=32:32:eng:image/png:icon32:/tmp/icon32.png +#hs20_icon=64:64:eng:image/png:icon64:/tmp/icon64.png + +# OSU SSID (see ssid2 for format description) +# This is the SSID used for all OSU connections to all the listed OSU Providers. +#osu_ssid="example" + +# OSU Providers +# One or more sets of following parameter. Each OSU provider is started by the +# mandatory osu_server_uri item. The other parameters add information for the +# last added OSU provider. +# +#osu_server_uri=https://example.com/osu/ +#osu_friendly_name=eng:Example operator +#osu_friendly_name=fin:Esimerkkipalveluntarjoaja +#osu_nai=anonymous@example.com +#osu_method_list=1 0 +#osu_icon=icon32 +#osu_icon=icon64 +#osu_service_desc=eng:Example services +#osu_service_desc=fin:Esimerkkipalveluja +# +#osu_server_uri=... + +##### TESTING OPTIONS ######################################################### +# +# The options in this section are only available when the build configuration +# option CONFIG_TESTING_OPTIONS is set while compiling hostapd. They allow +# testing some scenarios that are otherwise difficult to reproduce. +# +# Ignore probe requests sent to hostapd with the given probability, must be a +# floating point number in the range [0, 1). +#ignore_probe_probability=0.0 +# +# Ignore authentication frames with the given probability +#ignore_auth_probability=0.0 +# +# Ignore association requests with the given probability +#ignore_assoc_probability=0.0 +# +# Ignore reassociation requests with the given probability +#ignore_reassoc_probability=0.0 +# +# Corrupt Key MIC in GTK rekey EAPOL-Key frames with the given probability +#corrupt_gtk_rekey_mic_probability=0.0 + +##### Multiple BSSID support ################################################## +# +# Above configuration is using the default interface (wlan#, or multi-SSID VLAN +# interfaces). Other BSSIDs can be added by using separator 'bss' with +# default interface name to be allocated for the data packets of the new BSS. +# +# hostapd will generate BSSID mask based on the BSSIDs that are +# configured. hostapd will verify that dev_addr & MASK == dev_addr. If this is +# not the case, the MAC address of the radio must be changed before starting +# hostapd (ifconfig wlan0 hw ether ). If a BSSID is configured for +# every secondary BSS, this limitation is not applied at hostapd and other +# masks may be used if the driver supports them (e.g., swap the locally +# administered bit) +# +# BSSIDs are assigned in order to each BSS, unless an explicit BSSID is +# specified using the 'bssid' parameter. +# If an explicit BSSID is specified, it must be chosen such that it: +# - results in a valid MASK that covers it and the dev_addr +# - is not the same as the MAC address of the radio +# - is not the same as any other explicitly specified BSSID +# +# Not all drivers support multiple BSSes. The exact mechanism for determining +# the driver capabilities is driver specific. With the current (i.e., a recent +# kernel) drivers using nl80211, this information can be checked with "iw list" +# (search for "valid interface combinations"). +# +# Please note that hostapd uses some of the values configured for the first BSS +# as the defaults for the following BSSes. However, it is recommended that all +# BSSes include explicit configuration of all relevant configuration items. +# +#bss=wlan0_0 +#ssid=test2 +# most of the above items can be used here (apart from radio interface specific +# items, like channel) + +#bss=wlan0_1 +#bssid=00:13:10:95:fe:0b +# ... diff --git a/config/test_3dr_controller/etc/hostapd.conf.md5 b/config/test_3dr_controller/etc/hostapd.conf.md5 new file mode 100644 index 0000000..4f109fb --- /dev/null +++ b/config/test_3dr_controller/etc/hostapd.conf.md5 @@ -0,0 +1 @@ +a46c206ffafef83c91cebf497ea785b5 ./test_3dr_controller/etc/hostapd.conf diff --git a/config/test_3dr_controller/mnt/rootfs.ro/etc/hostapd.orig b/config/test_3dr_controller/mnt/rootfs.ro/etc/hostapd.orig new file mode 100644 index 0000000..def36b6 --- /dev/null +++ b/config/test_3dr_controller/mnt/rootfs.ro/etc/hostapd.orig @@ -0,0 +1,1749 @@ +##### hostapd configuration file ############################################## +# Empty lines and lines starting with # are ignored + +# AP netdevice name (without 'ap' postfix, i.e., wlan0 uses wlan0ap for +# management frames); ath0 for madwifi +interface=wlan0-ap + +# In case of madwifi, atheros, and nl80211 driver interfaces, an additional +# configuration parameter, bridge, may be used to notify hostapd if the +# interface is included in a bridge. This parameter is not used with Host AP +# driver. If the bridge parameter is not set, the drivers will automatically +# figure out the bridge interface (assuming sysfs is enabled and mounted to +# /sys) and this parameter may not be needed. +# +# For nl80211, this parameter can be used to request the AP interface to be +# added to the bridge automatically (brctl may refuse to do this before hostapd +# has been started to change the interface mode). If needed, the bridge +# interface is also created. +#bridge=br0 + +# Driver interface type (hostap/wired/madwifi/test/none/nl80211/bsd); +# default: hostap). nl80211 is used with all Linux mac80211 drivers. +# Use driver=none if building hostapd as a standalone RADIUS server that does +# not control any wireless/wired driver. +driver=nl80211 + +# hostapd event logger configuration +# +# Two output method: syslog and stdout (only usable if not forking to +# background). +# +# Module bitfield (ORed bitfield of modules that will be logged; -1 = all +# modules): +# bit 0 (1) = IEEE 802.11 +# bit 1 (2) = IEEE 802.1X +# bit 2 (4) = RADIUS +# bit 3 (8) = WPA +# bit 4 (16) = driver interface +# bit 5 (32) = IAPP +# bit 6 (64) = MLME +# +# Levels (minimum value for logged events): +# 0 = verbose debugging +# 1 = debugging +# 2 = informational messages +# 3 = notification +# 4 = warning +# +logger_syslog=-1 +logger_syslog_level=2 +logger_stdout=-1 +logger_stdout_level=2 + +# Interface for separate control program. If this is specified, hostapd +# will create this directory and a UNIX domain socket for listening to requests +# from external programs (CLI/GUI, etc.) for status information and +# configuration. The socket file will be named based on the interface name, so +# multiple hostapd processes/interfaces can be run at the same time if more +# than one interface is used. +# /var/run/hostapd is the recommended directory for sockets and by default, +# hostapd_cli will use it when trying to connect with hostapd. +ctrl_interface=/var/run/hostapd + +# Access control for the control interface can be configured by setting the +# directory to allow only members of a group to use sockets. This way, it is +# possible to run hostapd as root (since it needs to change network +# configuration and open raw sockets) and still allow GUI/CLI components to be +# run as non-root users. However, since the control interface can be used to +# change the network configuration, this access needs to be protected in many +# cases. By default, hostapd is configured to use gid 0 (root). If you +# want to allow non-root users to use the contron interface, add a new group +# and change this value to match with that group. Add users that should have +# control interface access to this group. +# +# This variable can be a group name or gid. +#ctrl_interface_group=wheel +ctrl_interface_group=0 + + +##### IEEE 802.11 related configuration ####################################### + +# SSID to be used in IEEE 802.11 management frames +ssid=SoloLink_Default +# Alternative formats for configuring SSID +# (double quoted string, hexdump, printf-escaped string) +#ssid2="test" +#ssid2=74657374 +#ssid2=P"hello\nthere" + +# UTF-8 SSID: Whether the SSID is to be interpreted using UTF-8 encoding +#utf8_ssid=1 + +# Country code (ISO/IEC 3166-1). Used to set regulatory domain. +# Set as needed to indicate country in which device is operating. +# This can limit available channels and transmit power. +country_code=US + +# Enable IEEE 802.11d. This advertises the country_code and the set of allowed +# channels and transmit power levels based on the regulatory limits. The +# country_code setting must be configured with the correct country for +# IEEE 802.11d functions. +# (default: 0 = disabled) +#ieee80211d=1 + +# Enable IEEE 802.11h. This enables radar detection and DFS support if +# available. DFS support is required on outdoor 5 GHz channels in most countries +# of the world. This can be used only with ieee80211d=1. +# (default: 0 = disabled) +#ieee80211h=1 + +# Add Power Constraint element to Beacon and Probe Response frames +# This config option adds Power Constraint element when applicable and Country +# element is added. Power Constraint element is required by Transmit Power +# Control. This can be used only with ieee80211d=1. +# Valid values are 0..255. +#local_pwr_constraint=3 + +# Set Spectrum Management subfield in the Capability Information field. +# This config option forces the Spectrum Management bit to be set. When this +# option is not set, the value of the Spectrum Management bit depends on whether +# DFS or TPC is required by regulatory authorities. This can be used only with +# ieee80211d=1 and local_pwr_constraint configured. +#spectrum_mgmt_required=1 + +# Operation mode (a = IEEE 802.11a, b = IEEE 802.11b, g = IEEE 802.11g, +# ad = IEEE 802.11ad (60 GHz); a/g options are used with IEEE 802.11n, too, to +# specify band) +# Default: IEEE 802.11b +hw_mode=g + +# Channel number (IEEE 802.11) +# (default: 0, i.e., not set) +# Please note that some drivers do not use this value from hostapd and the +# channel will need to be configured separately with iwconfig. +# +# If CONFIG_ACS build option is enabled, the channel can be selected +# automatically at run time by setting channel=acs_survey or channel=0, both of +# which will enable the ACS survey based algorithm. +channel=0 + +# ACS tuning - Automatic Channel Selection +# See: http://wireless.kernel.org/en/users/Documentation/acs +# +# You can customize the ACS survey algorithm with following variables: +# +# acs_num_scans requirement is 1..100 - number of scans to be performed that +# are used to trigger survey data gathering of an underlying device driver. +# Scans are passive and typically take a little over 100ms (depending on the +# driver) on each available channel for given hw_mode. Increasing this value +# means sacrificing startup time and gathering more data wrt channel +# interference that may help choosing a better channel. This can also help fine +# tune the ACS scan time in case a driver has different scan dwell times. +# +# Defaults: +acs_num_scans=3 + +# Channel list restriction. This option allows hostapd to select one of the +# provided channels when a channel should be automatically selected. This +# is currently only used for DFS when the current channels becomes unavailable +# due to radar interference, and is currently only useful when ieee80211h=1 is +# set. +# Default: not set (allow any enabled channel to be selected) +#chanlist=100 104 108 112 116 + +# Beacon interval in kus (1.024 ms) (default: 100; range 15..65535) +beacon_int=100 + +# DTIM (delivery traffic information message) period (range 1..255): +# number of beacons between DTIMs (1 = every beacon includes DTIM element) +# (default: 2) +dtim_period=2 + +# Maximum number of stations allowed in station table. New stations will be +# rejected after the station table is full. IEEE 802.11 has a limit of 2007 +# different association IDs, so this number should not be larger than that. +# (default: 2007) +max_num_sta=255 + +# RTS/CTS threshold; 2347 = disabled (default); range 0..2347 +# If this field is not included in hostapd.conf, hostapd will not control +# RTS threshold and 'iwconfig wlan# rts ' can be used to set it. +rts_threshold=2347 + +# Fragmentation threshold; 2346 = disabled (default); range 256..2346 +# If this field is not included in hostapd.conf, hostapd will not control +# fragmentation threshold and 'iwconfig wlan# frag ' can be used to set +# it. +fragm_threshold=2346 + +# Rate configuration +# Default is to enable all rates supported by the hardware. This configuration +# item allows this list be filtered so that only the listed rates will be left +# in the list. If the list is empty, all rates are used. This list can have +# entries that are not in the list of rates the hardware supports (such entries +# are ignored). The entries in this list are in 100 kbps, i.e., 11 Mbps = 110. +# If this item is present, at least one rate have to be matching with the rates +# hardware supports. +# default: use the most common supported rate setting for the selected +# hw_mode (i.e., this line can be removed from configuration file in most +# cases) +#supported_rates=10 20 55 110 60 90 120 180 240 360 480 540 + +# Basic rate set configuration +# List of rates (in 100 kbps) that are included in the basic rate set. +# If this item is not included, usually reasonable default set is used. +#basic_rates=10 20 +#basic_rates=10 20 55 110 +#basic_rates=60 120 240 + +# Short Preamble +# This parameter can be used to enable optional use of short preamble for +# frames sent at 2 Mbps, 5.5 Mbps, and 11 Mbps to improve network performance. +# This applies only to IEEE 802.11b-compatible networks and this should only be +# enabled if the local hardware supports use of short preamble. If any of the +# associated STAs do not support short preamble, use of short preamble will be +# disabled (and enabled when such STAs disassociate) dynamically. +# 0 = do not allow use of short preamble (default) +# 1 = allow use of short preamble +#preamble=1 + +# Station MAC address -based authentication +# Please note that this kind of access control requires a driver that uses +# hostapd to take care of management frame processing and as such, this can be +# used with driver=hostap or driver=nl80211, but not with driver=madwifi. +# 0 = accept unless in deny list +# 1 = deny unless in accept list +# 2 = use external RADIUS server (accept/deny lists are searched first) +macaddr_acl=0 + +# Accept/deny lists are read from separate files (containing list of +# MAC addresses, one per line). Use absolute path name to make sure that the +# files can be read on SIGHUP configuration reloads. +#accept_mac_file=/etc/hostapd.accept +#deny_mac_file=/etc/hostapd.deny + +# IEEE 802.11 specifies two authentication algorithms. hostapd can be +# configured to allow both of these or only one. Open system authentication +# should be used with IEEE 802.1X. +# Bit fields of allowed authentication algorithms: +# bit 0 = Open System Authentication +# bit 1 = Shared Key Authentication (requires WEP) +auth_algs=3 + +# Send empty SSID in beacons and ignore probe request frames that do not +# specify full SSID, i.e., require stations to know SSID. +# default: disabled (0) +# 1 = send empty (length=0) SSID in beacon and ignore probe request for +# broadcast SSID +# 2 = clear SSID (ASCII 0), but keep the original length (this may be required +# with some clients that do not support empty SSID) and ignore probe +# requests for broadcast SSID +ignore_broadcast_ssid=0 + +# Additional vendor specfic elements for Beacon and Probe Response frames +# This parameter can be used to add additional vendor specific element(s) into +# the end of the Beacon and Probe Response frames. The format for these +# element(s) is a hexdump of the raw information elements (id+len+payload for +# one or more elements) +#vendor_elements=dd0411223301 + +# TX queue parameters (EDCF / bursting) +# tx_queue__ +# queues: data0, data1, data2, data3, after_beacon, beacon +# (data0 is the highest priority queue) +# parameters: +# aifs: AIFS (default 2) +# cwmin: cwMin (1, 3, 7, 15, 31, 63, 127, 255, 511, 1023) +# cwmax: cwMax (1, 3, 7, 15, 31, 63, 127, 255, 511, 1023); cwMax >= cwMin +# burst: maximum length (in milliseconds with precision of up to 0.1 ms) for +# bursting +# +# Default WMM parameters (IEEE 802.11 draft; 11-03-0504-03-000e): +# These parameters are used by the access point when transmitting frames +# to the clients. +# +# Low priority / AC_BK = background +#tx_queue_data3_aifs=7 +#tx_queue_data3_cwmin=15 +#tx_queue_data3_cwmax=1023 +#tx_queue_data3_burst=0 +# Note: for IEEE 802.11b mode: cWmin=31 cWmax=1023 burst=0 +# +# Normal priority / AC_BE = best effort +#tx_queue_data2_aifs=3 +#tx_queue_data2_cwmin=15 +#tx_queue_data2_cwmax=63 +#tx_queue_data2_burst=0 +# Note: for IEEE 802.11b mode: cWmin=31 cWmax=127 burst=0 +# +# High priority / AC_VI = video +#tx_queue_data1_aifs=1 +#tx_queue_data1_cwmin=7 +#tx_queue_data1_cwmax=15 +#tx_queue_data1_burst=3.0 +# Note: for IEEE 802.11b mode: cWmin=15 cWmax=31 burst=6.0 +# +# Highest priority / AC_VO = voice +#tx_queue_data0_aifs=1 +#tx_queue_data0_cwmin=3 +#tx_queue_data0_cwmax=7 +#tx_queue_data0_burst=1.5 +# Note: for IEEE 802.11b mode: cWmin=7 cWmax=15 burst=3.3 + +# 802.1D Tag (= UP) to AC mappings +# WMM specifies following mapping of data frames to different ACs. This mapping +# can be configured using Linux QoS/tc and sch_pktpri.o module. +# 802.1D Tag 802.1D Designation Access Category WMM Designation +# 1 BK AC_BK Background +# 2 - AC_BK Background +# 0 BE AC_BE Best Effort +# 3 EE AC_BE Best Effort +# 4 CL AC_VI Video +# 5 VI AC_VI Video +# 6 VO AC_VO Voice +# 7 NC AC_VO Voice +# Data frames with no priority information: AC_BE +# Management frames: AC_VO +# PS-Poll frames: AC_BE + +# Default WMM parameters (IEEE 802.11 draft; 11-03-0504-03-000e): +# for 802.11a or 802.11g networks +# These parameters are sent to WMM clients when they associate. +# The parameters will be used by WMM clients for frames transmitted to the +# access point. +# +# note - txop_limit is in units of 32microseconds +# note - acm is admission control mandatory flag. 0 = admission control not +# required, 1 = mandatory +# note - here cwMin and cmMax are in exponent form. the actual cw value used +# will be (2^n)-1 where n is the value given here +# +wmm_enabled=1 +# +# WMM-PS Unscheduled Automatic Power Save Delivery [U-APSD] +# Enable this flag if U-APSD supported outside hostapd (eg., Firmware/driver) +#uapsd_advertisement_enabled=1 +# +# Low priority / AC_BK = background +wmm_ac_bk_cwmin=4 +wmm_ac_bk_cwmax=10 +wmm_ac_bk_aifs=7 +wmm_ac_bk_txop_limit=0 +wmm_ac_bk_acm=0 +# Note: for IEEE 802.11b mode: cWmin=5 cWmax=10 +# +# Normal priority / AC_BE = best effort +wmm_ac_be_aifs=3 +wmm_ac_be_cwmin=4 +wmm_ac_be_cwmax=10 +wmm_ac_be_txop_limit=0 +wmm_ac_be_acm=0 +# Note: for IEEE 802.11b mode: cWmin=5 cWmax=7 +# +# High priority / AC_VI = video +wmm_ac_vi_aifs=2 +wmm_ac_vi_cwmin=3 +wmm_ac_vi_cwmax=4 +wmm_ac_vi_txop_limit=94 +wmm_ac_vi_acm=0 +# Note: for IEEE 802.11b mode: cWmin=4 cWmax=5 txop_limit=188 +# +# Highest priority / AC_VO = voice +wmm_ac_vo_aifs=2 +wmm_ac_vo_cwmin=2 +wmm_ac_vo_cwmax=3 +wmm_ac_vo_txop_limit=47 +wmm_ac_vo_acm=0 +# Note: for IEEE 802.11b mode: cWmin=3 cWmax=4 burst=102 + +# Static WEP key configuration +# +# The key number to use when transmitting. +# It must be between 0 and 3, and the corresponding key must be set. +# default: not set +#wep_default_key=0 +# The WEP keys to use. +# A key may be a quoted string or unquoted hexadecimal digits. +# The key length should be 5, 13, or 16 characters, or 10, 26, or 32 +# digits, depending on whether 40-bit (64-bit), 104-bit (128-bit), or +# 128-bit (152-bit) WEP is used. +# Only the default key must be supplied; the others are optional. +# default: not set +#wep_key0=123456789a +#wep_key1="vwxyz" +#wep_key2=0102030405060708090a0b0c0d +#wep_key3=".2.4.6.8.0.23" + +# Station inactivity limit +# +# If a station does not send anything in ap_max_inactivity seconds, an +# empty data frame is sent to it in order to verify whether it is +# still in range. If this frame is not ACKed, the station will be +# disassociated and then deauthenticated. This feature is used to +# clear station table of old entries when the STAs move out of the +# range. +# +# The station can associate again with the AP if it is still in range; +# this inactivity poll is just used as a nicer way of verifying +# inactivity; i.e., client will not report broken connection because +# disassociation frame is not sent immediately without first polling +# the STA with a data frame. +# default: 300 (i.e., 5 minutes) +#ap_max_inactivity=300 +# +# The inactivity polling can be disabled to disconnect stations based on +# inactivity timeout so that idle stations are more likely to be disconnected +# even if they are still in range of the AP. This can be done by setting +# skip_inactivity_poll to 1 (default 0). +#skip_inactivity_poll=0 + +# Disassociate stations based on excessive transmission failures or other +# indications of connection loss. This depends on the driver capabilities and +# may not be available with all drivers. +#disassoc_low_ack=1 + +# Maximum allowed Listen Interval (how many Beacon periods STAs are allowed to +# remain asleep). Default: 65535 (no limit apart from field size) +#max_listen_interval=100 + +# WDS (4-address frame) mode with per-station virtual interfaces +# (only supported with driver=nl80211) +# This mode allows associated stations to use 4-address frames to allow layer 2 +# bridging to be used. +#wds_sta=1 + +# If bridge parameter is set, the WDS STA interface will be added to the same +# bridge by default. This can be overridden with the wds_bridge parameter to +# use a separate bridge. +#wds_bridge=wds-br0 + +# Start the AP with beaconing disabled by default. +#start_disabled=0 + +# Client isolation can be used to prevent low-level bridging of frames between +# associated stations in the BSS. By default, this bridging is allowed. +#ap_isolate=1 + +# Fixed BSS Load value for testing purposes +# This field can be used to configure hostapd to add a fixed BSS Load element +# into Beacon and Probe Response frames for testing purposes. The format is +# :: +#bss_load_test=12:80:20000 + +##### IEEE 802.11n related configuration ###################################### + +# ieee80211n: Whether IEEE 802.11n (HT) is enabled +# 0 = disabled (default) +# 1 = enabled +# Note: You will also need to enable WMM for full HT functionality. +ieee80211n=1 + +# ht_capab: HT capabilities (list of flags) +# LDPC coding capability: [LDPC] = supported +# Supported channel width set: [HT40-] = both 20 MHz and 40 MHz with secondary +# channel below the primary channel; [HT40+] = both 20 MHz and 40 MHz +# with secondary channel above the primary channel +# (20 MHz only if neither is set) +# Note: There are limits on which channels can be used with HT40- and +# HT40+. Following table shows the channels that may be available for +# HT40- and HT40+ use per IEEE 802.11n Annex J: +# freq HT40- HT40+ +# 2.4 GHz 5-13 1-7 (1-9 in Europe/Japan) +# 5 GHz 40,48,56,64 36,44,52,60 +# (depending on the location, not all of these channels may be available +# for use) +# Please note that 40 MHz channels may switch their primary and secondary +# channels if needed or creation of 40 MHz channel maybe rejected based +# on overlapping BSSes. These changes are done automatically when hostapd +# is setting up the 40 MHz channel. +# Spatial Multiplexing (SM) Power Save: [SMPS-STATIC] or [SMPS-DYNAMIC] +# (SMPS disabled if neither is set) +# HT-greenfield: [GF] (disabled if not set) +# Short GI for 20 MHz: [SHORT-GI-20] (disabled if not set) +# Short GI for 40 MHz: [SHORT-GI-40] (disabled if not set) +# Tx STBC: [TX-STBC] (disabled if not set) +# Rx STBC: [RX-STBC1] (one spatial stream), [RX-STBC12] (one or two spatial +# streams), or [RX-STBC123] (one, two, or three spatial streams); Rx STBC +# disabled if none of these set +# HT-delayed Block Ack: [DELAYED-BA] (disabled if not set) +# Maximum A-MSDU length: [MAX-AMSDU-7935] for 7935 octets (3839 octets if not +# set) +# DSSS/CCK Mode in 40 MHz: [DSSS_CCK-40] = allowed (not allowed if not set) +# 40 MHz intolerant [40-INTOLERANT] (not advertised if not set) +# L-SIG TXOP protection support: [LSIG-TXOP-PROT] (disabled if not set) +#ht_capab=[HT40-][SHORT-GI-20][SHORT-GI-40] + +# Require stations to support HT PHY (reject association if they do not) +#require_ht=1 + +# If set non-zero, require stations to perform scans of overlapping +# channels to test for stations which would be affected by 40 MHz traffic. +# This parameter sets the interval in seconds between these scans. This +# is useful only for testing that stations properly set the OBSS interval, +# since the other parameters in the OBSS scan parameters IE are set to 0. +#obss_interval=0 + +##### IEEE 802.11ac related configuration ##################################### + +# ieee80211ac: Whether IEEE 802.11ac (VHT) is enabled +# 0 = disabled (default) +# 1 = enabled +# Note: You will also need to enable WMM for full VHT functionality. +#ieee80211ac=1 + +# vht_capab: VHT capabilities (list of flags) +# +# vht_max_mpdu_len: [MAX-MPDU-7991] [MAX-MPDU-11454] +# Indicates maximum MPDU length +# 0 = 3895 octets (default) +# 1 = 7991 octets +# 2 = 11454 octets +# 3 = reserved +# +# supported_chan_width: [VHT160] [VHT160-80PLUS80] +# Indicates supported Channel widths +# 0 = 160 MHz & 80+80 channel widths are not supported (default) +# 1 = 160 MHz channel width is supported +# 2 = 160 MHz & 80+80 channel widths are supported +# 3 = reserved +# +# Rx LDPC coding capability: [RXLDPC] +# Indicates support for receiving LDPC coded pkts +# 0 = Not supported (default) +# 1 = Supported +# +# Short GI for 80 MHz: [SHORT-GI-80] +# Indicates short GI support for reception of packets transmitted with TXVECTOR +# params format equal to VHT and CBW = 80Mhz +# 0 = Not supported (default) +# 1 = Supported +# +# Short GI for 160 MHz: [SHORT-GI-160] +# Indicates short GI support for reception of packets transmitted with TXVECTOR +# params format equal to VHT and CBW = 160Mhz +# 0 = Not supported (default) +# 1 = Supported +# +# Tx STBC: [TX-STBC-2BY1] +# Indicates support for the transmission of at least 2x1 STBC +# 0 = Not supported (default) +# 1 = Supported +# +# Rx STBC: [RX-STBC-1] [RX-STBC-12] [RX-STBC-123] [RX-STBC-1234] +# Indicates support for the reception of PPDUs using STBC +# 0 = Not supported (default) +# 1 = support of one spatial stream +# 2 = support of one and two spatial streams +# 3 = support of one, two and three spatial streams +# 4 = support of one, two, three and four spatial streams +# 5,6,7 = reserved +# +# SU Beamformer Capable: [SU-BEAMFORMER] +# Indicates support for operation as a single user beamformer +# 0 = Not supported (default) +# 1 = Supported +# +# SU Beamformee Capable: [SU-BEAMFORMEE] +# Indicates support for operation as a single user beamformee +# 0 = Not supported (default) +# 1 = Supported +# +# Compressed Steering Number of Beamformer Antennas Supported: [BF-ANTENNA-2] +# Beamformee's capability indicating the maximum number of beamformer +# antennas the beamformee can support when sending compressed beamforming +# feedback +# If SU beamformer capable, set to maximum value minus 1 +# else reserved (default) +# +# Number of Sounding Dimensions: [SOUNDING-DIMENSION-2] +# Beamformer's capability indicating the maximum value of the NUM_STS parameter +# in the TXVECTOR of a VHT NDP +# If SU beamformer capable, set to maximum value minus 1 +# else reserved (default) +# +# MU Beamformer Capable: [MU-BEAMFORMER] +# Indicates support for operation as an MU beamformer +# 0 = Not supported or sent by Non-AP STA (default) +# 1 = Supported +# +# MU Beamformee Capable: [MU-BEAMFORMEE] +# Indicates support for operation as an MU beamformee +# 0 = Not supported or sent by AP (default) +# 1 = Supported +# +# VHT TXOP PS: [VHT-TXOP-PS] +# Indicates whether or not the AP supports VHT TXOP Power Save Mode +# or whether or not the STA is in VHT TXOP Power Save mode +# 0 = VHT AP doesnt support VHT TXOP PS mode (OR) VHT Sta not in VHT TXOP PS +# mode +# 1 = VHT AP supports VHT TXOP PS mode (OR) VHT Sta is in VHT TXOP power save +# mode +# +# +HTC-VHT Capable: [HTC-VHT] +# Indicates whether or not the STA supports receiving a VHT variant HT Control +# field. +# 0 = Not supported (default) +# 1 = supported +# +# Maximum A-MPDU Length Exponent: [MAX-A-MPDU-LEN-EXP0]..[MAX-A-MPDU-LEN-EXP7] +# Indicates the maximum length of A-MPDU pre-EOF padding that the STA can recv +# This field is an integer in the range of 0 to 7. +# The length defined by this field is equal to +# 2 pow(13 + Maximum A-MPDU Length Exponent) -1 octets +# +# VHT Link Adaptation Capable: [VHT-LINK-ADAPT2] [VHT-LINK-ADAPT3] +# Indicates whether or not the STA supports link adaptation using VHT variant +# HT Control field +# If +HTC-VHTcapable is 1 +# 0 = (no feedback) if the STA does not provide VHT MFB (default) +# 1 = reserved +# 2 = (Unsolicited) if the STA provides only unsolicited VHT MFB +# 3 = (Both) if the STA can provide VHT MFB in response to VHT MRQ and if the +# STA provides unsolicited VHT MFB +# Reserved if +HTC-VHTcapable is 0 +# +# Rx Antenna Pattern Consistency: [RX-ANTENNA-PATTERN] +# Indicates the possibility of Rx antenna pattern change +# 0 = Rx antenna pattern might change during the lifetime of an association +# 1 = Rx antenna pattern does not change during the lifetime of an association +# +# Tx Antenna Pattern Consistency: [TX-ANTENNA-PATTERN] +# Indicates the possibility of Tx antenna pattern change +# 0 = Tx antenna pattern might change during the lifetime of an association +# 1 = Tx antenna pattern does not change during the lifetime of an association +#vht_capab=[SHORT-GI-80][HTC-VHT] +# +# Require stations to support VHT PHY (reject association if they do not) +#require_vht=1 + +# 0 = 20 or 40 MHz operating Channel width +# 1 = 80 MHz channel width +# 2 = 160 MHz channel width +# 3 = 80+80 MHz channel width +#vht_oper_chwidth=1 +# +# center freq = 5 GHz + (5 * index) +# So index 42 gives center freq 5.210 GHz +# which is channel 42 in 5G band +# +#vht_oper_centr_freq_seg0_idx=42 +# +# center freq = 5 GHz + (5 * index) +# So index 159 gives center freq 5.795 GHz +# which is channel 159 in 5G band +# +#vht_oper_centr_freq_seg1_idx=159 + +##### IEEE 802.1X-2004 related configuration ################################## + +# Require IEEE 802.1X authorization +#ieee8021x=1 + +# IEEE 802.1X/EAPOL version +# hostapd is implemented based on IEEE Std 802.1X-2004 which defines EAPOL +# version 2. However, there are many client implementations that do not handle +# the new version number correctly (they seem to drop the frames completely). +# In order to make hostapd interoperate with these clients, the version number +# can be set to the older version (1) with this configuration value. +#eapol_version=2 + +# Optional displayable message sent with EAP Request-Identity. The first \0 +# in this string will be converted to ASCII-0 (nul). This can be used to +# separate network info (comma separated list of attribute=value pairs); see, +# e.g., RFC 4284. +#eap_message=hello +#eap_message=hello\0networkid=netw,nasid=foo,portid=0,NAIRealms=example.com + +# WEP rekeying (disabled if key lengths are not set or are set to 0) +# Key lengths for default/broadcast and individual/unicast keys: +# 5 = 40-bit WEP (also known as 64-bit WEP with 40 secret bits) +# 13 = 104-bit WEP (also known as 128-bit WEP with 104 secret bits) +#wep_key_len_broadcast=5 +#wep_key_len_unicast=5 +# Rekeying period in seconds. 0 = do not rekey (i.e., set keys only once) +#wep_rekey_period=300 + +# EAPOL-Key index workaround (set bit7) for WinXP Supplicant (needed only if +# only broadcast keys are used) +eapol_key_index_workaround=0 + +# EAP reauthentication period in seconds (default: 3600 seconds; 0 = disable +# reauthentication). +#eap_reauth_period=3600 + +# Use PAE group address (01:80:c2:00:00:03) instead of individual target +# address when sending EAPOL frames with driver=wired. This is the most common +# mechanism used in wired authentication, but it also requires that the port +# is only used by one station. +#use_pae_group_addr=1 + +##### Integrated EAP server ################################################### + +# Optionally, hostapd can be configured to use an integrated EAP server +# to process EAP authentication locally without need for an external RADIUS +# server. This functionality can be used both as a local authentication server +# for IEEE 802.1X/EAPOL and as a RADIUS server for other devices. + +# Use integrated EAP server instead of external RADIUS authentication +# server. This is also needed if hostapd is configured to act as a RADIUS +# authentication server. +eap_server=1 + +# Path for EAP server user database +# If SQLite support is included, this can be set to "sqlite:/path/to/sqlite.db" +# to use SQLite database instead of a text file. +#eap_user_file=/etc/hostapd.eap_user + +# CA certificate (PEM or DER file) for EAP-TLS/PEAP/TTLS +#ca_cert=/etc/hostapd.ca.pem + +# Server certificate (PEM or DER file) for EAP-TLS/PEAP/TTLS +#server_cert=/etc/hostapd.server.pem + +# Private key matching with the server certificate for EAP-TLS/PEAP/TTLS +# This may point to the same file as server_cert if both certificate and key +# are included in a single file. PKCS#12 (PFX) file (.p12/.pfx) can also be +# used by commenting out server_cert and specifying the PFX file as the +# private_key. +#private_key=/etc/hostapd.server.prv + +# Passphrase for private key +#private_key_passwd=secret passphrase + +# Server identity +# EAP methods that provide mechanism for authenticated server identity delivery +# use this value. If not set, "hostapd" is used as a default. +#server_id=server.example.com + +# Enable CRL verification. +# Note: hostapd does not yet support CRL downloading based on CDP. Thus, a +# valid CRL signed by the CA is required to be included in the ca_cert file. +# This can be done by using PEM format for CA certificate and CRL and +# concatenating these into one file. Whenever CRL changes, hostapd needs to be +# restarted to take the new CRL into use. +# 0 = do not verify CRLs (default) +# 1 = check the CRL of the user certificate +# 2 = check all CRLs in the certificate path +#check_crl=1 + +# Cached OCSP stapling response (DER encoded) +# If set, this file is sent as a certificate status response by the EAP server +# if the EAP peer requests certificate status in the ClientHello message. +# This cache file can be updated, e.g., by running following command +# periodically to get an update from the OCSP responder: +# openssl ocsp \ +# -no_nonce \ +# -CAfile /etc/hostapd.ca.pem \ +# -issuer /etc/hostapd.ca.pem \ +# -cert /etc/hostapd.server.pem \ +# -url http://ocsp.example.com:8888/ \ +# -respout /tmp/ocsp-cache.der +#ocsp_stapling_response=/tmp/ocsp-cache.der + +# dh_file: File path to DH/DSA parameters file (in PEM format) +# This is an optional configuration file for setting parameters for an +# ephemeral DH key exchange. In most cases, the default RSA authentication does +# not use this configuration. However, it is possible setup RSA to use +# ephemeral DH key exchange. In addition, ciphers with DSA keys always use +# ephemeral DH keys. This can be used to achieve forward secrecy. If the file +# is in DSA parameters format, it will be automatically converted into DH +# params. This parameter is required if anonymous EAP-FAST is used. +# You can generate DH parameters file with OpenSSL, e.g., +# "openssl dhparam -out /etc/hostapd.dh.pem 1024" +#dh_file=/etc/hostapd.dh.pem + +# Fragment size for EAP methods +#fragment_size=1400 + +# Finite cyclic group for EAP-pwd. Number maps to group of domain parameters +# using the IANA repository for IKE (RFC 2409). +#pwd_group=19 + +# Configuration data for EAP-SIM database/authentication gateway interface. +# This is a text string in implementation specific format. The example +# implementation in eap_sim_db.c uses this as the UNIX domain socket name for +# the HLR/AuC gateway (e.g., hlr_auc_gw). In this case, the path uses "unix:" +# prefix. If hostapd is built with SQLite support (CONFIG_SQLITE=y in .config), +# database file can be described with an optional db= parameter. +#eap_sim_db=unix:/tmp/hlr_auc_gw.sock +#eap_sim_db=unix:/tmp/hlr_auc_gw.sock db=/tmp/hostapd.db + +# Encryption key for EAP-FAST PAC-Opaque values. This key must be a secret, +# random value. It is configured as a 16-octet value in hex format. It can be +# generated, e.g., with the following command: +# od -tx1 -v -N16 /dev/random | colrm 1 8 | tr -d ' ' +#pac_opaque_encr_key=000102030405060708090a0b0c0d0e0f + +# EAP-FAST authority identity (A-ID) +# A-ID indicates the identity of the authority that issues PACs. The A-ID +# should be unique across all issuing servers. In theory, this is a variable +# length field, but due to some existing implementations requiring A-ID to be +# 16 octets in length, it is strongly recommended to use that length for the +# field to provid interoperability with deployed peer implementations. This +# field is configured in hex format. +#eap_fast_a_id=101112131415161718191a1b1c1d1e1f + +# EAP-FAST authority identifier information (A-ID-Info) +# This is a user-friendly name for the A-ID. For example, the enterprise name +# and server name in a human-readable format. This field is encoded as UTF-8. +#eap_fast_a_id_info=test server + +# Enable/disable different EAP-FAST provisioning modes: +#0 = provisioning disabled +#1 = only anonymous provisioning allowed +#2 = only authenticated provisioning allowed +#3 = both provisioning modes allowed (default) +#eap_fast_prov=3 + +# EAP-FAST PAC-Key lifetime in seconds (hard limit) +#pac_key_lifetime=604800 + +# EAP-FAST PAC-Key refresh time in seconds (soft limit on remaining hard +# limit). The server will generate a new PAC-Key when this number of seconds +# (or fewer) of the lifetime remains. +#pac_key_refresh_time=86400 + +# EAP-SIM and EAP-AKA protected success/failure indication using AT_RESULT_IND +# (default: 0 = disabled). +#eap_sim_aka_result_ind=1 + +# Trusted Network Connect (TNC) +# If enabled, TNC validation will be required before the peer is allowed to +# connect. Note: This is only used with EAP-TTLS and EAP-FAST. If any other +# EAP method is enabled, the peer will be allowed to connect without TNC. +#tnc=1 + + +##### IEEE 802.11f - Inter-Access Point Protocol (IAPP) ####################### + +# Interface to be used for IAPP broadcast packets +#iapp_interface=eth0 + + +##### RADIUS client configuration ############################################# +# for IEEE 802.1X with external Authentication Server, IEEE 802.11 +# authentication with external ACL for MAC addresses, and accounting + +# The own IP address of the access point (used as NAS-IP-Address) +own_ip_addr=127.0.0.1 + +# Optional NAS-Identifier string for RADIUS messages. When used, this should be +# a unique to the NAS within the scope of the RADIUS server. For example, a +# fully qualified domain name can be used here. +# When using IEEE 802.11r, nas_identifier must be set and must be between 1 and +# 48 octets long. +#nas_identifier=ap.example.com + +# RADIUS authentication server +#auth_server_addr=127.0.0.1 +#auth_server_port=1812 +#auth_server_shared_secret=secret + +# RADIUS accounting server +#acct_server_addr=127.0.0.1 +#acct_server_port=1813 +#acct_server_shared_secret=secret + +# Secondary RADIUS servers; to be used if primary one does not reply to +# RADIUS packets. These are optional and there can be more than one secondary +# server listed. +#auth_server_addr=127.0.0.2 +#auth_server_port=1812 +#auth_server_shared_secret=secret2 +# +#acct_server_addr=127.0.0.2 +#acct_server_port=1813 +#acct_server_shared_secret=secret2 + +# Retry interval for trying to return to the primary RADIUS server (in +# seconds). RADIUS client code will automatically try to use the next server +# when the current server is not replying to requests. If this interval is set, +# primary server will be retried after configured amount of time even if the +# currently used secondary server is still working. +#radius_retry_primary_interval=600 + + +# Interim accounting update interval +# If this is set (larger than 0) and acct_server is configured, hostapd will +# send interim accounting updates every N seconds. Note: if set, this overrides +# possible Acct-Interim-Interval attribute in Access-Accept message. Thus, this +# value should not be configured in hostapd.conf, if RADIUS server is used to +# control the interim interval. +# This value should not be less 600 (10 minutes) and must not be less than +# 60 (1 minute). +#radius_acct_interim_interval=600 + +# Request Chargeable-User-Identity (RFC 4372) +# This parameter can be used to configure hostapd to request CUI from the +# RADIUS server by including Chargeable-User-Identity attribute into +# Access-Request packets. +#radius_request_cui=1 + +# Dynamic VLAN mode; allow RADIUS authentication server to decide which VLAN +# is used for the stations. This information is parsed from following RADIUS +# attributes based on RFC 3580 and RFC 2868: Tunnel-Type (value 13 = VLAN), +# Tunnel-Medium-Type (value 6 = IEEE 802), Tunnel-Private-Group-ID (value +# VLANID as a string). Optionally, the local MAC ACL list (accept_mac_file) can +# be used to set static client MAC address to VLAN ID mapping. +# 0 = disabled (default) +# 1 = option; use default interface if RADIUS server does not include VLAN ID +# 2 = required; reject authentication if RADIUS server does not include VLAN ID +#dynamic_vlan=0 + +# VLAN interface list for dynamic VLAN mode is read from a separate text file. +# This list is used to map VLAN ID from the RADIUS server to a network +# interface. Each station is bound to one interface in the same way as with +# multiple BSSIDs or SSIDs. Each line in this text file is defining a new +# interface and the line must include VLAN ID and interface name separated by +# white space (space or tab). +# If no entries are provided by this file, the station is statically mapped +# to . interfaces. +#vlan_file=/etc/hostapd.vlan + +# Interface where 802.1q tagged packets should appear when a RADIUS server is +# used to determine which VLAN a station is on. hostapd creates a bridge for +# each VLAN. Then hostapd adds a VLAN interface (associated with the interface +# indicated by 'vlan_tagged_interface') and the appropriate wireless interface +# to the bridge. +#vlan_tagged_interface=eth0 + +# Bridge (prefix) to add the wifi and the tagged interface to. This gets the +# VLAN ID appended. It defaults to brvlan%d if no tagged interface is given +# and br%s.%d if a tagged interface is given, provided %s = tagged interface +# and %d = VLAN ID. +#vlan_bridge=brvlan + +# When hostapd creates a VLAN interface on vlan_tagged_interfaces, it needs +# to know how to name it. +# 0 = vlan, e.g., vlan1 +# 1 = ., e.g. eth0.1 +#vlan_naming=0 + +# Arbitrary RADIUS attributes can be added into Access-Request and +# Accounting-Request packets by specifying the contents of the attributes with +# the following configuration parameters. There can be multiple of these to +# add multiple attributes. These parameters can also be used to override some +# of the attributes added automatically by hostapd. +# Format: [:] +# attr_id: RADIUS attribute type (e.g., 26 = Vendor-Specific) +# syntax: s = string (UTF-8), d = integer, x = octet string +# value: attribute value in format indicated by the syntax +# If syntax and value parts are omitted, a null value (single 0x00 octet) is +# used. +# +# Additional Access-Request attributes +# radius_auth_req_attr=[:] +# Examples: +# Operator-Name = "Operator" +#radius_auth_req_attr=126:s:Operator +# Service-Type = Framed (2) +#radius_auth_req_attr=6:d:2 +# Connect-Info = "testing" (this overrides the automatically generated value) +#radius_auth_req_attr=77:s:testing +# Same Connect-Info value set as a hexdump +#radius_auth_req_attr=77:x:74657374696e67 + +# +# Additional Accounting-Request attributes +# radius_acct_req_attr=[:] +# Examples: +# Operator-Name = "Operator" +#radius_acct_req_attr=126:s:Operator + +# Dynamic Authorization Extensions (RFC 5176) +# This mechanism can be used to allow dynamic changes to user session based on +# commands from a RADIUS server (or some other disconnect client that has the +# needed session information). For example, Disconnect message can be used to +# request an associated station to be disconnected. +# +# This is disabled by default. Set radius_das_port to non-zero UDP port +# number to enable. +#radius_das_port=3799 +# +# DAS client (the host that can send Disconnect/CoA requests) and shared secret +#radius_das_client=192.168.1.123 shared secret here +# +# DAS Event-Timestamp time window in seconds +#radius_das_time_window=300 +# +# DAS require Event-Timestamp +#radius_das_require_event_timestamp=1 + +##### RADIUS authentication server configuration ############################## + +# hostapd can be used as a RADIUS authentication server for other hosts. This +# requires that the integrated EAP server is also enabled and both +# authentication services are sharing the same configuration. + +# File name of the RADIUS clients configuration for the RADIUS server. If this +# commented out, RADIUS server is disabled. +#radius_server_clients=/etc/hostapd.radius_clients + +# The UDP port number for the RADIUS authentication server +#radius_server_auth_port=1812 + +# The UDP port number for the RADIUS accounting server +# Commenting this out or setting this to 0 can be used to disable RADIUS +# accounting while still enabling RADIUS authentication. +#radius_server_acct_port=1813 + +# Use IPv6 with RADIUS server (IPv4 will also be supported using IPv6 API) +#radius_server_ipv6=1 + + +##### WPA/IEEE 802.11i configuration ########################################## + +# Enable WPA. Setting this variable configures the AP to require WPA (either +# WPA-PSK or WPA-RADIUS/EAP based on other configuration). For WPA-PSK, either +# wpa_psk or wpa_passphrase must be set and wpa_key_mgmt must include WPA-PSK. +# Instead of wpa_psk / wpa_passphrase, wpa_psk_radius might suffice. +# For WPA-RADIUS/EAP, ieee8021x must be set (but without dynamic WEP keys), +# RADIUS authentication server must be configured, and WPA-EAP must be included +# in wpa_key_mgmt. +# This field is a bit field that can be used to enable WPA (IEEE 802.11i/D3.0) +# and/or WPA2 (full IEEE 802.11i/RSN): +# bit0 = WPA +# bit1 = IEEE 802.11i/RSN (WPA2) (dot11RSNAEnabled) +wpa=2 + +# WPA pre-shared keys for WPA-PSK. This can be either entered as a 256-bit +# secret in hex format (64 hex digits), wpa_psk, or as an ASCII passphrase +# (8..63 characters) that will be converted to PSK. This conversion uses SSID +# so the PSK changes when ASCII passphrase is used and the SSID is changed. +# wpa_psk (dot11RSNAConfigPSKValue) +# wpa_passphrase (dot11RSNAConfigPSKPassPhrase) +#wpa_psk=0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef +wpa_passphrase=sololink + +# Optionally, WPA PSKs can be read from a separate text file (containing list +# of (PSK,MAC address) pairs. This allows more than one PSK to be configured. +# Use absolute path name to make sure that the files can be read on SIGHUP +# configuration reloads. +#wpa_psk_file=/etc/hostapd.wpa_psk + +# Optionally, WPA passphrase can be received from RADIUS authentication server +# This requires macaddr_acl to be set to 2 (RADIUS) +# 0 = disabled (default) +# 1 = optional; use default passphrase/psk if RADIUS server does not include +# Tunnel-Password +# 2 = required; reject authentication if RADIUS server does not include +# Tunnel-Password +#wpa_psk_radius=0 + +# Set of accepted key management algorithms (WPA-PSK, WPA-EAP, or both). The +# entries are separated with a space. WPA-PSK-SHA256 and WPA-EAP-SHA256 can be +# added to enable SHA256-based stronger algorithms. +# (dot11RSNAConfigAuthenticationSuitesTable) +#wpa_key_mgmt=WPA-PSK WPA-EAP + +# Set of accepted cipher suites (encryption algorithms) for pairwise keys +# (unicast packets). This is a space separated list of algorithms: +# CCMP = AES in Counter mode with CBC-MAC [RFC 3610, IEEE 802.11i/D7.0] +# TKIP = Temporal Key Integrity Protocol [IEEE 802.11i/D7.0] +# Group cipher suite (encryption algorithm for broadcast and multicast frames) +# is automatically selected based on this configuration. If only CCMP is +# allowed as the pairwise cipher, group cipher will also be CCMP. Otherwise, +# TKIP will be used as the group cipher. +# (dot11RSNAConfigPairwiseCiphersTable) +# Pairwise cipher for WPA (v1) (default: TKIP) +wpa_pairwise=TKIP CCMP +# Pairwise cipher for RSN/WPA2 (default: use wpa_pairwise value) +#rsn_pairwise=CCMP + +# Time interval for rekeying GTK (broadcast/multicast encryption keys) in +# seconds. (dot11RSNAConfigGroupRekeyTime) +#wpa_group_rekey=600 + +# Rekey GTK when any STA that possesses the current GTK is leaving the BSS. +# (dot11RSNAConfigGroupRekeyStrict) +#wpa_strict_rekey=1 + +# Time interval for rekeying GMK (master key used internally to generate GTKs +# (in seconds). +#wpa_gmk_rekey=86400 + +# Maximum lifetime for PTK in seconds. This can be used to enforce rekeying of +# PTK to mitigate some attacks against TKIP deficiencies. +#wpa_ptk_rekey=600 + +# Enable IEEE 802.11i/RSN/WPA2 pre-authentication. This is used to speed up +# roaming be pre-authenticating IEEE 802.1X/EAP part of the full RSN +# authentication and key handshake before actually associating with a new AP. +# (dot11RSNAPreauthenticationEnabled) +#rsn_preauth=1 +# +# Space separated list of interfaces from which pre-authentication frames are +# accepted (e.g., 'eth0' or 'eth0 wlan0wds0'. This list should include all +# interface that are used for connections to other APs. This could include +# wired interfaces and WDS links. The normal wireless data interface towards +# associated stations (e.g., wlan0) should not be added, since +# pre-authentication is only used with APs other than the currently associated +# one. +#rsn_preauth_interfaces=eth0 + +# peerkey: Whether PeerKey negotiation for direct links (IEEE 802.11e) is +# allowed. This is only used with RSN/WPA2. +# 0 = disabled (default) +# 1 = enabled +#peerkey=1 + +# ieee80211w: Whether management frame protection (MFP) is enabled +# 0 = disabled (default) +# 1 = optional +# 2 = required +#ieee80211w=0 + +# Group management cipher suite +# Default: AES-128-CMAC (BIP) +# Other options (depending on driver support): +# BIP-GMAC-128 +# BIP-GMAC-256 +# BIP-CMAC-256 +# Note: All the stations connecting to the BSS will also need to support the +# selected cipher. The default AES-128-CMAC is the only option that is commonly +# available in deployed devices. +#group_mgmt_cipher=AES-128-CMAC + +# Association SA Query maximum timeout (in TU = 1.024 ms; for MFP) +# (maximum time to wait for a SA Query response) +# dot11AssociationSAQueryMaximumTimeout, 1...4294967295 +#assoc_sa_query_max_timeout=1000 + +# Association SA Query retry timeout (in TU = 1.024 ms; for MFP) +# (time between two subsequent SA Query requests) +# dot11AssociationSAQueryRetryTimeout, 1...4294967295 +#assoc_sa_query_retry_timeout=201 + +# disable_pmksa_caching: Disable PMKSA caching +# This parameter can be used to disable caching of PMKSA created through EAP +# authentication. RSN preauthentication may still end up using PMKSA caching if +# it is enabled (rsn_preauth=1). +# 0 = PMKSA caching enabled (default) +# 1 = PMKSA caching disabled +#disable_pmksa_caching=0 + +# okc: Opportunistic Key Caching (aka Proactive Key Caching) +# Allow PMK cache to be shared opportunistically among configured interfaces +# and BSSes (i.e., all configurations within a single hostapd process). +# 0 = disabled (default) +# 1 = enabled +#okc=1 + +# SAE threshold for anti-clogging mechanism (dot11RSNASAEAntiCloggingThreshold) +# This parameter defines how many open SAE instances can be in progress at the +# same time before the anti-clogging mechanism is taken into use. +#sae_anti_clogging_threshold=5 + +# Enabled SAE finite cyclic groups +# SAE implementation are required to support group 19 (ECC group defined over a +# 256-bit prime order field). All groups that are supported by the +# implementation are enabled by default. This configuration parameter can be +# used to specify a limited set of allowed groups. The group values are listed +# in the IANA registry: +# http://www.iana.org/assignments/ipsec-registry/ipsec-registry.xml#ipsec-registry-9 +#sae_groups=19 20 21 25 26 + +##### IEEE 802.11r configuration ############################################## + +# Mobility Domain identifier (dot11FTMobilityDomainID, MDID) +# MDID is used to indicate a group of APs (within an ESS, i.e., sharing the +# same SSID) between which a STA can use Fast BSS Transition. +# 2-octet identifier as a hex string. +#mobility_domain=a1b2 + +# PMK-R0 Key Holder identifier (dot11FTR0KeyHolderID) +# 1 to 48 octet identifier. +# This is configured with nas_identifier (see RADIUS client section above). + +# Default lifetime of the PMK-RO in minutes; range 1..65535 +# (dot11FTR0KeyLifetime) +#r0_key_lifetime=10000 + +# PMK-R1 Key Holder identifier (dot11FTR1KeyHolderID) +# 6-octet identifier as a hex string. +#r1_key_holder=000102030405 + +# Reassociation deadline in time units (TUs / 1.024 ms; range 1000..65535) +# (dot11FTReassociationDeadline) +#reassociation_deadline=1000 + +# List of R0KHs in the same Mobility Domain +# format: <128-bit key as hex string> +# This list is used to map R0KH-ID (NAS Identifier) to a destination MAC +# address when requesting PMK-R1 key from the R0KH that the STA used during the +# Initial Mobility Domain Association. +#r0kh=02:01:02:03:04:05 r0kh-1.example.com 000102030405060708090a0b0c0d0e0f +#r0kh=02:01:02:03:04:06 r0kh-2.example.com 00112233445566778899aabbccddeeff +# And so on.. One line per R0KH. + +# List of R1KHs in the same Mobility Domain +# format: <128-bit key as hex string> +# This list is used to map R1KH-ID to a destination MAC address when sending +# PMK-R1 key from the R0KH. This is also the list of authorized R1KHs in the MD +# that can request PMK-R1 keys. +#r1kh=02:01:02:03:04:05 02:11:22:33:44:55 000102030405060708090a0b0c0d0e0f +#r1kh=02:01:02:03:04:06 02:11:22:33:44:66 00112233445566778899aabbccddeeff +# And so on.. One line per R1KH. + +# Whether PMK-R1 push is enabled at R0KH +# 0 = do not push PMK-R1 to all configured R1KHs (default) +# 1 = push PMK-R1 to all configured R1KHs whenever a new PMK-R0 is derived +#pmk_r1_push=1 + +##### Neighbor table ########################################################## +# Maximum number of entries kept in AP table (either for neigbor table or for +# detecting Overlapping Legacy BSS Condition). The oldest entry will be +# removed when adding a new entry that would make the list grow over this +# limit. Note! WFA certification for IEEE 802.11g requires that OLBC is +# enabled, so this field should not be set to 0 when using IEEE 802.11g. +# default: 255 +#ap_table_max_size=255 + +# Number of seconds of no frames received after which entries may be deleted +# from the AP table. Since passive scanning is not usually performed frequently +# this should not be set to very small value. In addition, there is no +# guarantee that every scan cycle will receive beacon frames from the +# neighboring APs. +# default: 60 +#ap_table_expiration_time=3600 + + +##### Wi-Fi Protected Setup (WPS) ############################################# + +# WPS state +# 0 = WPS disabled (default) +# 1 = WPS enabled, not configured +# 2 = WPS enabled, configured +wps_state=2 + +# Whether to manage this interface independently from other WPS interfaces +# By default, a single hostapd process applies WPS operations to all configured +# interfaces. This parameter can be used to disable that behavior for a subset +# of interfaces. If this is set to non-zero for an interface, WPS commands +# issued on that interface do not apply to other interfaces and WPS operations +# performed on other interfaces do not affect this interface. +#wps_independent=0 + +# AP can be configured into a locked state where new WPS Registrar are not +# accepted, but previously authorized Registrars (including the internal one) +# can continue to add new Enrollees. +#ap_setup_locked=1 + +# Universally Unique IDentifier (UUID; see RFC 4122) of the device +# This value is used as the UUID for the internal WPS Registrar. If the AP +# is also using UPnP, this value should be set to the device's UPnP UUID. +# If not configured, UUID will be generated based on the local MAC address. +#uuid=12345678-9abc-def0-1234-56789abcdef0 + +# Note: If wpa_psk_file is set, WPS is used to generate random, per-device PSKs +# that will be appended to the wpa_psk_file. If wpa_psk_file is not set, the +# default PSK (wpa_psk/wpa_passphrase) will be delivered to Enrollees. Use of +# per-device PSKs is recommended as the more secure option (i.e., make sure to +# set wpa_psk_file when using WPS with WPA-PSK). + +# When an Enrollee requests access to the network with PIN method, the Enrollee +# PIN will need to be entered for the Registrar. PIN request notifications are +# sent to hostapd ctrl_iface monitor. In addition, they can be written to a +# text file that could be used, e.g., to populate the AP administration UI with +# pending PIN requests. If the following variable is set, the PIN requests will +# be written to the configured file. +#wps_pin_requests=/var/run/hostapd_wps_pin_requests + +# Device Name +# User-friendly description of device; up to 32 octets encoded in UTF-8 +#device_name=Wireless AP + +# Manufacturer +# The manufacturer of the device (up to 64 ASCII characters) +#manufacturer=Company + +# Model Name +# Model of the device (up to 32 ASCII characters) +#model_name=WAP + +# Model Number +# Additional device description (up to 32 ASCII characters) +#model_number=123 + +# Serial Number +# Serial number of the device (up to 32 characters) +#serial_number=12345 + +# Primary Device Type +# Used format: -- +# categ = Category as an integer value +# OUI = OUI and type octet as a 4-octet hex-encoded value; 0050F204 for +# default WPS OUI +# subcateg = OUI-specific Sub Category as an integer value +# Examples: +# 1-0050F204-1 (Computer / PC) +# 1-0050F204-2 (Computer / Server) +# 5-0050F204-1 (Storage / NAS) +# 6-0050F204-1 (Network Infrastructure / AP) +#device_type=6-0050F204-1 + +# OS Version +# 4-octet operating system version number (hex string) +#os_version=01020300 + +# Config Methods +# List of the supported configuration methods +# Available methods: usba ethernet label display ext_nfc_token int_nfc_token +# nfc_interface push_button keypad virtual_display physical_display +# virtual_push_button physical_push_button +#config_methods=label virtual_display virtual_push_button keypad + +# WPS capability discovery workaround for PBC with Windows 7 +# Windows 7 uses incorrect way of figuring out AP's WPS capabilities by acting +# as a Registrar and using M1 from the AP. The config methods attribute in that +# message is supposed to indicate only the configuration method supported by +# the AP in Enrollee role, i.e., to add an external Registrar. For that case, +# PBC shall not be used and as such, the PushButton config method is removed +# from M1 by default. If pbc_in_m1=1 is included in the configuration file, +# the PushButton config method is left in M1 (if included in config_methods +# parameter) to allow Windows 7 to use PBC instead of PIN (e.g., from a label +# in the AP). +#pbc_in_m1=1 + +# Static access point PIN for initial configuration and adding Registrars +# If not set, hostapd will not allow external WPS Registrars to control the +# access point. The AP PIN can also be set at runtime with hostapd_cli +# wps_ap_pin command. Use of temporary (enabled by user action) and random +# AP PIN is much more secure than configuring a static AP PIN here. As such, +# use of the ap_pin parameter is not recommended if the AP device has means for +# displaying a random PIN. +#ap_pin=12345670 + +# Skip building of automatic WPS credential +# This can be used to allow the automatically generated Credential attribute to +# be replaced with pre-configured Credential(s). +#skip_cred_build=1 + +# Additional Credential attribute(s) +# This option can be used to add pre-configured Credential attributes into M8 +# message when acting as a Registrar. If skip_cred_build=1, this data will also +# be able to override the Credential attribute that would have otherwise been +# automatically generated based on network configuration. This configuration +# option points to an external file that much contain the WPS Credential +# attribute(s) as binary data. +#extra_cred=hostapd.cred + +# Credential processing +# 0 = process received credentials internally (default) +# 1 = do not process received credentials; just pass them over ctrl_iface to +# external program(s) +# 2 = process received credentials internally and pass them over ctrl_iface +# to external program(s) +# Note: With wps_cred_processing=1, skip_cred_build should be set to 1 and +# extra_cred be used to provide the Credential data for Enrollees. +# +# wps_cred_processing=1 will disabled automatic updates of hostapd.conf file +# both for Credential processing and for marking AP Setup Locked based on +# validation failures of AP PIN. An external program is responsible on updating +# the configuration appropriately in this case. +#wps_cred_processing=0 + +# AP Settings Attributes for M7 +# By default, hostapd generates the AP Settings Attributes for M7 based on the +# current configuration. It is possible to override this by providing a file +# with pre-configured attributes. This is similar to extra_cred file format, +# but the AP Settings attributes are not encapsulated in a Credential +# attribute. +#ap_settings=hostapd.ap_settings + +# WPS UPnP interface +# If set, support for external Registrars is enabled. +#upnp_iface=br0 + +# Friendly Name (required for UPnP) +# Short description for end use. Should be less than 64 characters. +#friendly_name=WPS Access Point + +# Manufacturer URL (optional for UPnP) +#manufacturer_url=http://www.example.com/ + +# Model Description (recommended for UPnP) +# Long description for end user. Should be less than 128 characters. +#model_description=Wireless Access Point + +# Model URL (optional for UPnP) +#model_url=http://www.example.com/model/ + +# Universal Product Code (optional for UPnP) +# 12-digit, all-numeric code that identifies the consumer package. +#upc=123456789012 + +# WPS RF Bands (a = 5G, b = 2.4G, g = 2.4G, ag = dual band) +# This value should be set according to RF band(s) supported by the AP if +# hw_mode is not set. For dual band dual concurrent devices, this needs to be +# set to ag to allow both RF bands to be advertized. +#wps_rf_bands=ag + +# NFC password token for WPS +# These parameters can be used to configure a fixed NFC password token for the +# AP. This can be generated, e.g., with nfc_pw_token from wpa_supplicant. When +# these parameters are used, the AP is assumed to be deployed with a NFC tag +# that includes the matching NFC password token (e.g., written based on the +# NDEF record from nfc_pw_token). +# +#wps_nfc_dev_pw_id: Device Password ID (16..65535) +#wps_nfc_dh_pubkey: Hexdump of DH Public Key +#wps_nfc_dh_privkey: Hexdump of DH Private Key +#wps_nfc_dev_pw: Hexdump of Device Password + +##### Wi-Fi Direct (P2P) ###################################################### + +# Enable P2P Device management +#manage_p2p=1 + +# Allow cross connection +#allow_cross_connection=1 + +#### TDLS (IEEE 802.11z-2010) ################################################# + +# Prohibit use of TDLS in this BSS +#tdls_prohibit=1 + +# Prohibit use of TDLS Channel Switching in this BSS +#tdls_prohibit_chan_switch=1 + +##### IEEE 802.11v-2011 ####################################################### + +# Time advertisement +# 0 = disabled (default) +# 2 = UTC time at which the TSF timer is 0 +#time_advertisement=2 + +# Local time zone as specified in 8.3 of IEEE Std 1003.1-2004: +# stdoffset[dst[offset][,start[/time],end[/time]]] +#time_zone=EST5 + +# WNM-Sleep Mode (extended sleep mode for stations) +# 0 = disabled (default) +# 1 = enabled (allow stations to use WNM-Sleep Mode) +#wnm_sleep_mode=1 + +# BSS Transition Management +# 0 = disabled (default) +# 1 = enabled +#bss_transition=1 + +##### IEEE 802.11u-2011 ####################################################### + +# Enable Interworking service +#interworking=1 + +# Access Network Type +# 0 = Private network +# 1 = Private network with guest access +# 2 = Chargeable public network +# 3 = Free public network +# 4 = Personal device network +# 5 = Emergency services only network +# 14 = Test or experimental +# 15 = Wildcard +#access_network_type=0 + +# Whether the network provides connectivity to the Internet +# 0 = Unspecified +# 1 = Network provides connectivity to the Internet +#internet=1 + +# Additional Step Required for Access +# Note: This is only used with open network, i.e., ASRA shall ne set to 0 if +# RSN is used. +#asra=0 + +# Emergency services reachable +#esr=0 + +# Unauthenticated emergency service accessible +#uesa=0 + +# Venue Info (optional) +# The available values are defined in IEEE Std 802.11u-2011, 7.3.1.34. +# Example values (group,type): +# 0,0 = Unspecified +# 1,7 = Convention Center +# 1,13 = Coffee Shop +# 2,0 = Unspecified Business +# 7,1 Private Residence +#venue_group=7 +#venue_type=1 + +# Homogeneous ESS identifier (optional; dot11HESSID) +# If set, this shall be identifical to one of the BSSIDs in the homogeneous +# ESS and this shall be set to the same value across all BSSs in homogeneous +# ESS. +#hessid=02:03:04:05:06:07 + +# Roaming Consortium List +# Arbitrary number of Roaming Consortium OIs can be configured with each line +# adding a new OI to the list. The first three entries are available through +# Beacon and Probe Response frames. Any additional entry will be available only +# through ANQP queries. Each OI is between 3 and 15 octets and is configured as +# a hexstring. +#roaming_consortium=021122 +#roaming_consortium=2233445566 + +# Venue Name information +# This parameter can be used to configure one or more Venue Name Duples for +# Venue Name ANQP information. Each entry has a two or three character language +# code (ISO-639) separated by colon from the venue name string. +# Note that venue_group and venue_type have to be set for Venue Name +# information to be complete. +#venue_name=eng:Example venue +#venue_name=fin:Esimerkkipaikka +# Alternative format for language:value strings: +# (double quoted string, printf-escaped string) +#venue_name=P"eng:Example\nvenue" + +# Network Authentication Type +# This parameter indicates what type of network authentication is used in the +# network. +# format: [redirect URL] +# Network Authentication Type Indicator values: +# 00 = Acceptance of terms and conditions +# 01 = On-line enrollment supported +# 02 = http/https redirection +# 03 = DNS redirection +#network_auth_type=00 +#network_auth_type=02http://www.example.com/redirect/me/here/ + +# IP Address Type Availability +# format: <1-octet encoded value as hex str> +# (ipv4_type & 0x3f) << 2 | (ipv6_type & 0x3) +# ipv4_type: +# 0 = Address type not available +# 1 = Public IPv4 address available +# 2 = Port-restricted IPv4 address available +# 3 = Single NATed private IPv4 address available +# 4 = Double NATed private IPv4 address available +# 5 = Port-restricted IPv4 address and single NATed IPv4 address available +# 6 = Port-restricted IPv4 address and double NATed IPv4 address available +# 7 = Availability of the address type is not known +# ipv6_type: +# 0 = Address type not available +# 1 = Address type available +# 2 = Availability of the address type not known +#ipaddr_type_availability=14 + +# Domain Name +# format: [,] +#domain_name=example.com,another.example.com,yet-another.example.com + +# 3GPP Cellular Network information +# format: [;][;...] +#anqp_3gpp_cell_net=244,91;310,026;234,56 + +# NAI Realm information +# One or more realm can be advertised. Each nai_realm line adds a new realm to +# the set. These parameters provide information for stations using Interworking +# network selection to allow automatic connection to a network based on +# credentials. +# format: ,[,][,][,...] +# encoding: +# 0 = Realm formatted in accordance with IETF RFC 4282 +# 1 = UTF-8 formatted character string that is not formatted in +# accordance with IETF RFC 4282 +# NAI Realm(s): Semi-colon delimited NAI Realm(s) +# EAP Method: [:<[AuthParam1:Val1]>][<[AuthParam2:Val2]>][...] +# EAP Method types, see: +# http://www.iana.org/assignments/eap-numbers/eap-numbers.xhtml#eap-numbers-4 +# AuthParam (Table 8-188 in IEEE Std 802.11-2012): +# ID 2 = Non-EAP Inner Authentication Type +# 1 = PAP, 2 = CHAP, 3 = MSCHAP, 4 = MSCHAPV2 +# ID 3 = Inner authentication EAP Method Type +# ID 5 = Credential Type +# 1 = SIM, 2 = USIM, 3 = NFC Secure Element, 4 = Hardware Token, +# 5 = Softoken, 6 = Certificate, 7 = username/password, 9 = Anonymous, +# 10 = Vendor Specific +#nai_realm=0,example.com;example.net +# EAP methods EAP-TLS with certificate and EAP-TTLS/MSCHAPv2 with +# username/password +#nai_realm=0,example.org,13[5:6],21[2:4][5:7] + +# QoS Map Set configuration +# +# Comma delimited QoS Map Set in decimal values +# (see IEEE Std 802.11-2012, 8.4.2.97) +# +# format: +# [,],... +# +# There can be up to 21 optional DSCP Exceptions which are pairs of DSCP Value +# (0..63 or 255) and User Priority (0..7). This is followed by eight DSCP Range +# descriptions with DSCP Low Value and DSCP High Value pairs (0..63 or 255) for +# each UP starting from 0. If both low and high value are set to 255, the +# corresponding UP is not used. +# +# default: not set +#qos_map_set=53,2,22,6,8,15,0,7,255,255,16,31,32,39,255,255,40,47,255,255 + +##### Hotspot 2.0 ############################################################# + +# Enable Hotspot 2.0 support +#hs20=1 + +# Disable Downstream Group-Addressed Forwarding (DGAF) +# This can be used to configure a network where no group-addressed frames are +# allowed. The AP will not forward any group-address frames to the stations and +# random GTKs are issued for each station to prevent associated stations from +# forging such frames to other stations in the BSS. +#disable_dgaf=1 + +# OSU Server-Only Authenticated L2 Encryption Network +#osen=1 + +# ANQP Domain ID (0..65535) +# An identifier for a set of APs in an ESS that share the same common ANQP +# information. 0 = Some of the ANQP information is unique to this AP (default). +#anqp_domain_id=1234 + +# Deauthentication request timeout +# If the RADIUS server indicates that the station is not allowed to connect to +# the BSS/ESS, the AP can allow the station some time to download a +# notification page (URL included in the message). This parameter sets that +# timeout in seconds. +#hs20_deauth_req_timeout=60 + +# Operator Friendly Name +# This parameter can be used to configure one or more Operator Friendly Name +# Duples. Each entry has a two or three character language code (ISO-639) +# separated by colon from the operator friendly name string. +#hs20_oper_friendly_name=eng:Example operator +#hs20_oper_friendly_name=fin:Esimerkkioperaattori + +# Connection Capability +# This can be used to advertise what type of IP traffic can be sent through the +# hotspot (e.g., due to firewall allowing/blocking protocols/ports). +# format: :: +# IP Protocol: 1 = ICMP, 6 = TCP, 17 = UDP +# Port Number: 0..65535 +# Status: 0 = Closed, 1 = Open, 2 = Unknown +# Each hs20_conn_capab line is added to the list of advertised tuples. +#hs20_conn_capab=1:0:2 +#hs20_conn_capab=6:22:1 +#hs20_conn_capab=17:5060:0 + +# WAN Metrics +# format: :
      :
        :
        :
          : +# WAN Info: B0-B1: Link Status, B2: Symmetric Link, B3: At Capabity +# (encoded as two hex digits) +# Link Status: 1 = Link up, 2 = Link down, 3 = Link in test state +# Downlink Speed: Estimate of WAN backhaul link current downlink speed in kbps; +# 1..4294967295; 0 = unknown +# Uplink Speed: Estimate of WAN backhaul link current uplink speed in kbps +# 1..4294967295; 0 = unknown +# Downlink Load: Current load of downlink WAN connection (scaled to 255 = 100%) +# Uplink Load: Current load of uplink WAN connection (scaled to 255 = 100%) +# Load Measurement Duration: Duration for measuring downlink/uplink load in +# tenths of a second (1..65535); 0 if load cannot be determined +#hs20_wan_metrics=01:8000:1000:80:240:3000 + +# Operating Class Indication +# List of operating classes the BSSes in this ESS use. The Global operating +# classes in Table E-4 of IEEE Std 802.11-2012 Annex E define the values that +# can be used in this. +# format: hexdump of operating class octets +# for example, operating classes 81 (2.4 GHz channels 1-13) and 115 (5 GHz +# channels 36-48): +#hs20_operating_class=5173 + +# OSU icons +# ::::: +#hs20_icon=32:32:eng:image/png:icon32:/tmp/icon32.png +#hs20_icon=64:64:eng:image/png:icon64:/tmp/icon64.png + +# OSU SSID (see ssid2 for format description) +# This is the SSID used for all OSU connections to all the listed OSU Providers. +#osu_ssid="example" + +# OSU Providers +# One or more sets of following parameter. Each OSU provider is started by the +# mandatory osu_server_uri item. The other parameters add information for the +# last added OSU provider. +# +#osu_server_uri=https://example.com/osu/ +#osu_friendly_name=eng:Example operator +#osu_friendly_name=fin:Esimerkkipalveluntarjoaja +#osu_nai=anonymous@example.com +#osu_method_list=1 0 +#osu_icon=icon32 +#osu_icon=icon64 +#osu_service_desc=eng:Example services +#osu_service_desc=fin:Esimerkkipalveluja +# +#osu_server_uri=... + +##### TESTING OPTIONS ######################################################### +# +# The options in this section are only available when the build configuration +# option CONFIG_TESTING_OPTIONS is set while compiling hostapd. They allow +# testing some scenarios that are otherwise difficult to reproduce. +# +# Ignore probe requests sent to hostapd with the given probability, must be a +# floating point number in the range [0, 1). +#ignore_probe_probability=0.0 +# +# Ignore authentication frames with the given probability +#ignore_auth_probability=0.0 +# +# Ignore association requests with the given probability +#ignore_assoc_probability=0.0 +# +# Ignore reassociation requests with the given probability +#ignore_reassoc_probability=0.0 +# +# Corrupt Key MIC in GTK rekey EAPOL-Key frames with the given probability +#corrupt_gtk_rekey_mic_probability=0.0 + +##### Multiple BSSID support ################################################## +# +# Above configuration is using the default interface (wlan#, or multi-SSID VLAN +# interfaces). Other BSSIDs can be added by using separator 'bss' with +# default interface name to be allocated for the data packets of the new BSS. +# +# hostapd will generate BSSID mask based on the BSSIDs that are +# configured. hostapd will verify that dev_addr & MASK == dev_addr. If this is +# not the case, the MAC address of the radio must be changed before starting +# hostapd (ifconfig wlan0 hw ether ). If a BSSID is configured for +# every secondary BSS, this limitation is not applied at hostapd and other +# masks may be used if the driver supports them (e.g., swap the locally +# administered bit) +# +# BSSIDs are assigned in order to each BSS, unless an explicit BSSID is +# specified using the 'bssid' parameter. +# If an explicit BSSID is specified, it must be chosen such that it: +# - results in a valid MASK that covers it and the dev_addr +# - is not the same as the MAC address of the radio +# - is not the same as any other explicitly specified BSSID +# +# Not all drivers support multiple BSSes. The exact mechanism for determining +# the driver capabilities is driver specific. With the current (i.e., a recent +# kernel) drivers using nl80211, this information can be checked with "iw list" +# (search for "valid interface combinations"). +# +# Please note that hostapd uses some of the values configured for the first BSS +# as the defaults for the following BSSes. However, it is recommended that all +# BSSes include explicit configuration of all relevant configuration items. +# +#bss=wlan0_0 +#ssid=test2 +# most of the above items can be used here (apart from radio interface specific +# items, like channel) + +#bss=wlan0_1 +#bssid=00:13:10:95:fe:0b +# ... diff --git a/config/test_3dr_solo/PIX_VERSION b/config/test_3dr_solo/PIX_VERSION new file mode 100644 index 0000000..1fe6958 --- /dev/null +++ b/config/test_3dr_solo/PIX_VERSION @@ -0,0 +1 @@ +0.0.28 diff --git a/config/test_3dr_solo/VERSION b/config/test_3dr_solo/VERSION new file mode 100644 index 0000000..ef5e445 --- /dev/null +++ b/config/test_3dr_solo/VERSION @@ -0,0 +1 @@ +0.6.5 diff --git a/config/test_3dr_solo/etc/wpa_supplicant.conf b/config/test_3dr_solo/etc/wpa_supplicant.conf new file mode 100644 index 0000000..8ede844 --- /dev/null +++ b/config/test_3dr_solo/etc/wpa_supplicant.conf @@ -0,0 +1,6 @@ +ctrl_interface=/var/run/wpa_supplicant +ctrl_interface_group=0 +update_config=1 +device_name=Solo +manufacturer=3D Robotics +model_name=Solo diff --git a/config/test_3dr_solo/etc/wpa_supplicant.conf.md5 b/config/test_3dr_solo/etc/wpa_supplicant.conf.md5 new file mode 100644 index 0000000..57f9610 --- /dev/null +++ b/config/test_3dr_solo/etc/wpa_supplicant.conf.md5 @@ -0,0 +1 @@ +132d9621948cfb385c5975b51dec4758 ./test_3dr_solo/etc/wpa_supplicant.conf diff --git a/config/test_3dr_solo/mnt/rootfs.ro/etc/wpa_supplicant.orig b/config/test_3dr_solo/mnt/rootfs.ro/etc/wpa_supplicant.orig new file mode 100644 index 0000000..8ede844 --- /dev/null +++ b/config/test_3dr_solo/mnt/rootfs.ro/etc/wpa_supplicant.orig @@ -0,0 +1,6 @@ +ctrl_interface=/var/run/wpa_supplicant +ctrl_interface_group=0 +update_config=1 +device_name=Solo +manufacturer=3D Robotics +model_name=Solo diff --git a/config/test_configinit b/config/test_configinit new file mode 100755 index 0000000..cddf8d7 --- /dev/null +++ b/config/test_configinit @@ -0,0 +1,244 @@ +#!/bin/sh + +verbose= + +# arguments +# -v verbose +until [ -z "$1" ]; do + case "$1" in + -v) verbose="-v" ;; + esac + shift +done + +# This runs on Ubuntu +MD5SUM='md5sum -c --status' + +config_names="hostapd wpa_supplicant sololink" + +valid() { + [ -f ${1} ] && [ -f ${1}.md5 ] && ${MD5SUM} ${1}.md5 +} + +assert_valid() { + if ! valid ${1}; then + echo "##### TEST FAILED ##### (valid ${1})" + fi +} + +assert_same() { + if ! cmp ${1} ${2}; then + echo "##### TEST FAILED ##### (cmp ${1} ${2})" + fi +} + +test_first_boot() { + echo + echo "##### test_first_boot starting" + echo + + # delete all + mkdir -p etc/ + rm -f etc/* + + # only .orig files exist + for config in ${config_names}; do + cp rootfs.ro/etc/${config}.orig etc/${config}.orig + chmod +w etc/${config}.orig + done + + ./configinit ${verbose} + + # should have: + # * .base, .base.md5 + # * .conf, .conf.md5 + # * .orig, .orig.md5 + # * .base, .conf, .orig should all be the same + # * all .md5 should be good + for config in ${config_names}; do + assert_valid etc/${config}.base + assert_valid etc/${config}.conf + assert_valid etc/${config}.orig + assert_same etc/${config}.orig etc/${config}.base + assert_same etc/${config}.orig etc/${config}.conf + done + + echo + echo "##### test_first_boot complete" + echo +} + +test_normal_boot() { + echo + echo "##### test_normal_boot starting" + echo + + # delete all + mkdir -p etc/ + rm -f etc/* + + # .orig, .base, and .conf files exist + for config in ${config_names}; do + cp rootfs.ro/etc/${config}.orig etc/${config}.orig + chmod +w etc/${config}.orig + md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5 + cp etc/${config}.orig etc/${config}.base + md5sum `realpath etc/${config}.base` > etc/${config}.base.md5 + cp etc/${config}.orig etc/${config}.conf + # .conf is normally changed from .orig + echo "# added line!" >> etc/${config}.conf + md5sum `realpath etc/${config}.conf` > etc/${config}.conf.md5 + done + + ./configinit ${verbose} + + # should have: + # * .base, .base.md5 + # * .conf, .conf.md5 + # * .orig, .orig.md5 + # * .base and .orig should be the same + # * all .md5 should be good + for config in ${config_names}; do + assert_valid etc/${config}.base + assert_valid etc/${config}.conf + assert_valid etc/${config}.orig + assert_same etc/${config}.orig etc/${config}.base + done + + echo + echo "##### test_normal_boot complete" + echo +} + +test_invalid_conf() { + echo + echo "##### test_invalid_conf starting" + echo + + # delete all + mkdir -p etc/ + rm -f etc/* + + # .orig, .base, and .conf files exist + for config in ${config_names}; do + cp rootfs.ro/etc/${config}.orig etc/${config}.orig + chmod +w etc/${config}.orig + md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5 + cp etc/${config}.orig etc/${config}.base + md5sum `realpath etc/${config}.base` > etc/${config}.base.md5 + cp etc/${config}.orig etc/${config}.conf + # incomplete update of .conf + cp etc/${config}.conf etc/${config}.back + md5sum `realpath etc/${config}.back` > etc/${config}.back.md5 + echo "# added line!" >> etc/${config}.conf + # .conf.md5 is now invalid + done + + ./configinit ${verbose} + + # should have: + # * .base, .base.md5 + # * .conf, .conf.md5 + # * .orig, .orig.md5 + # * .base and .orig should be the same + # * all .md5 should be good + for config in ${config_names}; do + assert_valid etc/${config}.base + assert_valid etc/${config}.conf + assert_valid etc/${config}.orig + assert_same etc/${config}.orig etc/${config}.base + done + + echo + echo "##### test_invalid_conf complete" + echo +} + +test_update() { + echo + echo "##### test_update starting" + echo + + # delete all + mkdir -p etc/ + rm -f etc/* + + # .orig, .base, and .conf files exist + for config in ${config_names}; do + cp rootfs.ro/etc/${config}.orig etc/${config}.orig + chmod +w etc/${config}.orig + md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5 + cp etc/${config}.orig etc/${config}.base + md5sum `realpath etc/${config}.base` > etc/${config}.base.md5 + cp etc/${config}.orig etc/${config}.conf + md5sum `realpath etc/${config}.conf` > etc/${config}.conf.md5 + done + + # prior to the update, hostapd.conf and wpa_supplicant.conf had ssid changed + + sed -i "s/^ssid=SoloLink_.*/ssid=SoloLink_Test/" etc/hostapd.conf + md5sum `realpath etc/hostapd.conf` > etc/hostapd.conf.md5 + + echo "network={" >> etc/wpa_supplicant.conf + echo " ssid=\"SoloLink_Test\"" >> etc/wpa_supplicant.conf + echo " key_mgmt=NONE" >> etc/wpa_supplicant.conf + echo "}" >> etc/wpa_supplicant.conf + md5sum `realpath etc/wpa_supplicant.conf` > etc/wpa_supplicant.conf.md5 + + # telemetry baud was changed in sololink.conf + sed -i "s/^telemBaud=.*/telemBaud=115200/" etc/sololink.conf + md5sum `realpath etc/sololink.conf` > etc/sololink.conf.md5 + + # as part of update, we change something in hostapd.orig + sed -i "s/^acs_num_scans=.*/acs_num_scans=1000/" etc/hostapd.orig + + # we do not change wpa_supplicant.orig + + # we add a new setting to sololink.orig + sed -i "s/useGpsTime=True/useGpsTime=True\n\nuseOuijiTime=Definitely/" etc/sololink.orig + + # .orig.md5 is no longer correct for the updated versions + # (that is how the update is detected) + + # show changes + for config in ${config_names}; do + echo "user changes in ${config}.conf:" + diff etc/${config}.base etc/${config}.conf + echo + echo "update changes in ${config}.orig:" + diff rootfs.ro/etc/${config}.orig etc/${config}.orig + echo + done + + ./configinit ${verbose} + + # should have: + # * .base, .base.md5 + # * .conf, .conf.md5 + # * .orig, .orig.md5 + # * .base and .orig should be the same, no user changes + # * .conf should have both update and user changes + for config in ${config_names}; do + assert_valid etc/${config}.base + assert_valid etc/${config}.conf + assert_valid etc/${config}.orig + assert_same etc/${config}.orig etc/${config}.base + echo "${config}.conf should have both user and update mods:" + diff rootfs.ro/etc/${config}.orig etc/${config}.conf + done + + echo + echo "##### test_update complete" + echo +} + +test_first_boot + +test_normal_boot + +test_invalid_conf + +test_update + +# clean up +rm -rf ./etc diff --git a/flightcode/Makefile b/flightcode/Makefile new file mode 100644 index 0000000..7ad124c --- /dev/null +++ b/flightcode/Makefile @@ -0,0 +1,51 @@ + +SUBDIRS = +SUBDIRS += arp_table +SUBDIRS += dflog +SUBDIRS += dataflash_logger +SUBDIRS += hostapd_ctrl +SUBDIRS += pixrc +SUBDIRS += proc_top +SUBDIRS += rssi +SUBDIRS += stm32 +SUBDIRS += telem +SUBDIRS += telem_ctrl +SUBDIRS += tlog +SUBDIRS += video/vid +SUBDIRS += video/app +SUBDIRS += video/hdmi +SUBDIRS += unlock +SUBDIRS += wdog + +SUBDIRS_BUILD = $(SUBDIRS:%=%_build) +SUBDIRS_CLEAN = $(SUBDIRS:%=%_clean) + +# SUBDIRS2 is SUBDIRS, plus directories that can be formatted but not built +SUBDIRS2 = $(SUBDIRS) +SUBDIRS2 += ini +SUBDIRS2 += ini/cpp +SUBDIRS2 += log +SUBDIRS2 += util + +SUBDIRS_FMT = $(SUBDIRS2:%=%_fmt) +SUBDIRS_FMT_DIFF = $(SUBDIRS2:%=%_fmt-diff) + +all: $(SUBDIRS_BUILD) + +build: $(SUBDIRS_BUILD) +$(SUBDIRS_BUILD): + $(MAKE) -C $(@:%_build=%) + +clean: $(SUBDIRS_CLEAN) +$(SUBDIRS_CLEAN): + $(MAKE) -C $(@:%_clean=%) clean + +fmt: $(SUBDIRS_FMT) +$(SUBDIRS_FMT): + $(MAKE) -C $(@:%_fmt=%) fmt + +fmt-diff: $(SUBDIRS_FMT_DIFF) +$(SUBDIRS_FMT_DIFF): + $(MAKE) -C $(@:%_fmt-diff=%) fmt-diff + +.PHONY: $(SUBDIRS) $(SUBDIRS_BUILD) $(SUBDIRS_CLEAN) $(SUBDIRS_FMT) $(SUBDIRS_FMT_DIFF) diff --git a/flightcode/arp_table/Makefile b/flightcode/arp_table/Makefile new file mode 100644 index 0000000..01f39f3 --- /dev/null +++ b/flightcode/arp_table/Makefile @@ -0,0 +1,40 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) + +SRCS_C = main.c arp_table.c util.c + +OBJS = $(SRCS_C:.c=.o) + +MAIN = arp_table + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.c) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/arp_table/main.c b/flightcode/arp_table/main.c new file mode 100644 index 0000000..ed21244 --- /dev/null +++ b/flightcode/arp_table/main.c @@ -0,0 +1,33 @@ + +#include +#include +#include +#include +#include "arp_table.h" + +#define MAX_ARP_ENTRIES 10 +arp_entry_t arp_table[MAX_ARP_ENTRIES]; + +int main(int argc, char *argv[]) +{ + int arp_entries; + int i; + + arp_entries = MAX_ARP_ENTRIES; + if (arp_table_get(arp_table, &arp_entries) != 0) { + printf("ERROR reading arp table\n"); + exit(1); + } + + for (i = 0; i < arp_entries; i++) { + struct in_addr in; + in.s_addr = arp_table[i].ip; + uint8_t *mac = arp_table[i].mac; + printf("%-15s 0x%x 0x%x %02x:%02x:%02x:%02x:%02x:%02x %s\n", inet_ntoa(in), + arp_table[i].hw_type, arp_table[i].flags, mac[0], mac[1], mac[2], mac[3], mac[4], + mac[5], arp_table[i].dev); + } + + exit(0); + +} /* main */ diff --git a/flightcode/baudcheck/Makefile b/flightcode/baudcheck/Makefile new file mode 100644 index 0000000..ce8e5cb --- /dev/null +++ b/flightcode/baudcheck/Makefile @@ -0,0 +1,37 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS += -lpthread + +SRCS_CPP = baudcheck.cpp +SRCS_C += util.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = baudcheck + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + $(RM) ../util/*.o + +.PHONY: clean diff --git a/flightcode/baudcheck/baudcheck.cpp b/flightcode/baudcheck/baudcheck.cpp new file mode 100644 index 0000000..f6dc543 --- /dev/null +++ b/flightcode/baudcheck/baudcheck.cpp @@ -0,0 +1,371 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "../mavlink/c_library/common/mavlink.h" +#include +#include +#include +#include +#include + +using namespace std; + +/*********************************************************************** +Serial port name +***********************************************************************/ +string serialPortName; + +/*********************************************************************** +File descriptors +***********************************************************************/ +int serial_fd; + +/*********************************************************************** +Threading variables +***********************************************************************/ +pthread_mutex_t mutex_msg; +pthread_mutex_t mutex_msgtype; +mavlink_message_t latest_msg; +sem_t sem_msg; +int msg_type = 0; + +/*********************************************************************** +Serial tx/rx buffer size +***********************************************************************/ +#define BUFSIZE 256 + +/*********************************************************************** +Function: int serial_setup(int baud) + +Description: The serial port initialization function. This function + initializes the serial port over which DSM data is sent to + the pixhawk. A return of 0 indicates an error. +***********************************************************************/ +int serial_setup(int baud) +{ + struct termios options; + + serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY); + + if(serial_fd < 0) + { + cerr << "Unable to open serial port " << serialPortName.c_str() << endl; + return 0; + } + + tcflush(serial_fd, TCIOFLUSH); + + //Configure port for 8N1 transmission + tcgetattr(serial_fd, &options); //Gets the current options for the port + //Set the output baud rate + switch(baud) + { + case 57600: cfsetspeed(&options, B57600); break; + case 115200: cfsetspeed(&options, B115200); break; + case 1500000: cfsetspeed(&options, B1500000); break; + default: + cerr << "Unsupported baud rate" << endl; + return 0; + } + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; + options.c_cflag |= CRTSCTS; + options.c_cc[VMIN] = 17; + options.c_cc[VTIME] = 0; + + tcsetattr(serial_fd, TCSANOW, &options); //Set the new options for the port "NOW" + + //sleep(1); + tcflush(serial_fd, TCIOFLUSH); + + cout << "Opened serial port " << serialPortName.c_str() << endl; + + return 1; +} + +void connectUSB(void) +{ + system("echo 19 >> /sys/class/gpio/export"); + system("echo 21 >> /sys/class/gpio/export"); + system("echo out >> /sys/class/gpio/gpio19/direction"); + system("echo out >> /sys/class/gpio/gpio21/direction"); + system("echo 1 >> /sys/class/gpio/gpio21/value"); + system("echo 0 >> /sys/class/gpio/gpio19/value"); + +} + +void disconnectUSB(void) +{ + system("echo 0 >> /sys/class/gpio/gpio21/value"); + system("echo 0 >> /sys/class/gpio/gpio19/value"); + system("echo 19 >> /sys/class/gpio/unexport"); + system("echo 21 >> /sys/class/gpio/unexport"); +} + +/*********************************************************************** +Function: int mavlink_task() + +Description: Thread task to pull mavlink data from the serial port + and dump it into a mutex-protected mavlink message. + + When log data begins streaming, dumps log data into the + log file on the filesystem. +***********************************************************************/ +void *mavlink_task(void*) +{ + mavlink_message_t _msg; + mavlink_status_t mavlink_status; + int read_len; + int i; + char buf[BUFSIZE]; + int msgtolookfor; + + while(1) + { + read_len = read(serial_fd, buf, BUFSIZE); + + if (read_len < 0) + { + if (errno != EAGAIN) + cerr << "Read err: " << errno << endl; + } + + if(read_len > 0) + { + for(i=0; id_name, usbdev_string, 23)) + { + foundDevice = true; + strcpy(usbdev_string,direntry->d_name); + break; + } + } + + if(!foundDevice) + { + cerr << "Unable to find USB device" << endl; + return -1; + } + + closedir(dir); + + //Serial port setup + serialPortName = "/dev/serial/by-id/"; + serialPortName.append(usbdev_string); + if(!serial_setup(115200)) + { + cerr << "Unable to initialize the USB serial port" << endl; + return -1; + } + + return 0; +} + +/*********************************************************************** +Function: void request_baudrate() + +Description: Request a particular log file from the pixhawk. +***********************************************************************/ +void request_baudrate(void) +{ + char buf[BUFSIZE]; + mavlink_message_t msg; + int msg_len; + float baud; + int tries; + bool gotParam=false; + + mavlink_msg_param_request_read_pack(1, 1, &msg, 1, 1, "SERIAL1_BAUD", -1); + msg_len = mavlink_msg_to_send_buffer((uint8_t*)buf,&msg); + + if(write(serial_fd, buf, msg_len) != msg_len) + { + cerr << "Serial port write error." << endl; + disconnectUSB(); + exit(1); + } + + for(tries=0; tries<3; ++tries) + { + cout << "Requesting SERIAL1_BAUD..." << endl; + + if(wait_for_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, 3) >= 0) + { + baud = mavlink_msg_param_value_get_param_value(&msg); + cout << "Got param value " << baud << endl; + disconnectUSB(); + exit(0); + } + } + + if(!gotParam) + { + cout << "Unable to get SERIAL1_BAUD_VALUE" << endl; + disconnectUSB(); + exit(1); + } +} + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(int argc, char *argv[]) +{ + int baudrate; + pthread_t mavlink_ctx; + mavlink_message_t msg; + + //Cout cleanup + std::cout.precision(2); + std::cout.setf( ios::fixed, ios::floatfield ); + + cout << "Pixhawk baudrate checker" << endl; + + //Serial port setup + serialPortName = "/dev/ttymxc1"; + baudrate=115200;//57600; + if(!serial_setup(baudrate)) + { + cerr << "Unable to initialize the serial send" << endl; + return -1; + } + + //Threading initialization + pthread_mutex_init(&mutex_msg, NULL); + sem_init(&sem_msg, 0, 0); + + //Start the mavlink rx thread + pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL); + + cout << "Waiting for a hearbeat..." << endl; + + //Wait for a heartbeat on the telem port + if(wait_for_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 5) < 0) + { + cout << "No heartbeat received, requesting baudrate from USB" << endl; + pthread_cancel(mavlink_ctx); + close(serial_fd); + if (open_USB_serial() < 0) + return -1; + } + else + { + cout << "Got a heartbeat, exiting" << endl; + return 0; + } + + pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL); + + //Send a param request to the pixhawk + request_baudrate(); + + //Wait for the mavlink thread to end + pthread_join(mavlink_ctx, NULL); + + return 0; +} diff --git a/flightcode/command/Commander.cpp b/flightcode/command/Commander.cpp new file mode 100644 index 0000000..aea1c6a --- /dev/null +++ b/flightcode/command/Commander.cpp @@ -0,0 +1,370 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "mutex.h" +#include "Commander.h" + +using namespace std; + + +// Constructor creates and opens the command socket, then starts the command +// thread. +Commander::Commander(const char *sock_name) +{ + + if (strlen(sock_name) >= sizeof(sa.sun_path)) { + cerr << "ERROR: command socket name too long" << endl; + return; + } + + // If there is something already there with the socket name, it is deleted. + // If not, unlink() fails (and we don't care). + (void)unlink(sock_name); + + // create command socket + _fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (_fd < 0) { + cerr << "ERROR creating command socket" << endl; + return; + } + + // bind command socket to address + memset(&sa, 0, sizeof(sa)); + sa.sun_family = AF_UNIX; + strncpy(sa.sun_path, sock_name, sizeof(sa.sun_path) - 1); + if (bind(_fd, (struct sockaddr *)&sa, sizeof(sa)) != 0) { + cerr << "ERROR binding command socket" << endl; + close(_fd); + _fd = -1; + return; + } + + // initialize clients list mutex + if (mutex_init(&clients_mutex) != 0) { + cerr << "ERROR initializing clients mutex" << endl; + close(_fd); + _fd = -1; + return; + } + + // start command thread + if (pthread_create(&thread_id, NULL, &Commander::command_entry, this) != 0) { + cerr << "ERROR starting command thread" << endl; + close(_fd); + _fd = -1; + return; + } + +} // Commander::Commander + + +// Destructor sends a QUIT command to the command thread, waits for it to exit, +// then closes the command socket. +Commander::~Commander() +{ + const char *quit = "QUIT"; + ssize_t quit_len = strlen(quit); + + if (sendto(_fd, quit, quit_len, 0, + (const struct sockaddr *)&sa, sizeof(sa)) != quit_len) { + cerr << "~Commander: ERROR returned from sendto" << endl; + } + + if (pthread_join(thread_id, NULL) != 0) { + cerr << "ERROR returned from pthread_join" << endl; + } + + close(_fd); + +} // Commander::~Commander() + + +// Commander processing thread. +// Wait for a command on the command socket, and process each. A QUIT command +// causes the loop to exit. +void *Commander::command_thread(void) +{ + bool quit = false; + char buf[MAX_CMD_LEN]; + int nb; + struct sockaddr_storage src_storage; + struct sockaddr *src = (struct sockaddr *)&src_storage; + socklen_t src_len; + char *token; + const char *delims; + + //cout << "command_thread: running" << endl; + + while (!quit) { + + src_len = sizeof(struct sockaddr_storage); + memset(src, 0, src_len); + memset(&buf, 0, sizeof(buf)); + nb = recvfrom(_fd, buf, MAX_CMD_LEN, 0, src, &src_len); + if (nb < 0) { + cerr << "ERROR returned from recvfrom" << endl; + continue; + } + + delims = " \t\r\n"; + token = strtok(buf, delims); + + if (token == NULL) { + // no command, like ping + handle_ping(src, src_len); + } else if (strcasecmp(token, "ATTACH") == 0) { + token = strtok(NULL, delims); + handle_attach(src, src_len, token, true); + } else if (strcasecmp(token, "DETACH") == 0) { + token = strtok(NULL, delims); + handle_attach(src, src_len, token, false); + } else if (strcasecmp(token, "PING") == 0) { + handle_ping(src, src_len); + } else if (strcasecmp(token, "LIST") == 0) { + handle_list(src, src_len); + } else if (strcasecmp(token, "QUIT") == 0) { + quit = true; + } else { + cerr << "Unknown command: " << buf << endl; + } + + } // while (!quit) + + //cout << "command_thread: exiting" << endl; + + return NULL; + +} // Commander::command_thread + + +// Send data to all clients. +// The supplied data is sent to all clients in the client list, i.e. all +// clients for which an ATTACH command has been sent. Unix domain datagram +// sockets are reliable, which means a write will block if the reader is +// not fast enough. We can't let an RC packet consumer block the UDP thread, +// so the sendto is done nonblocking. This means it is up to the reader to +// read fast enough to keep from dropping packets. +void Commander::send_clients(const void *data, int data_len) +{ + static unsigned drops = 0; + vector::iterator it; + + pthread_mutex_lock(&clients_mutex); + + for (it = clients.begin(); it != clients.end(); it++) { + if (sendto(_fd, data, data_len, MSG_DONTWAIT, + it->sockaddr(), it->socklen()) != data_len) { + //cerr << "send_clients: \"" << strerror(errno) + // << "\" sending to " << *it << endl; + drops++; + } else { + //cout << "send_clients: sent to " << *it << endl; + if (drops > 0) { + syslog(LOG_INFO, "cmd: dropped %d packets", drops); + drops = 0; + } + } + } + + pthread_mutex_unlock(&clients_mutex); + +} // Commander::send_clients + + +// Handle ATTACH or DETACH command. +// +// ATTACH/DETACH takes and optional argument indicating the client to ATTACH or +// detach. If no argument is given, the sender of the command is used. If an +// argument is given, it should be an AF_UNIX socket (support for UDP sockets +// could be added). +// +// If the command was ATTACH and the client is not in the list of clients, add +// it to the list. If the command was DETACH and the client is in the list of +// clients, remove it from the list. +void Commander::handle_attach(const struct sockaddr *src_addr, + socklen_t src_addr_len, const char *client, + bool attach) +{ + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + + memset(&client_addr, 0, sizeof(client_addr)); + client_addr_len = 0; + + if (client == NULL) { + // no client given - use source of request as client + memcpy(&client_addr, src_addr, src_addr_len); + client_addr_len = src_addr_len; + } else { + // client should be a unix socket name + // (support for IP:port could be added) + struct stat stat_buf; + if (stat(client, &stat_buf) != 0) { + cerr << "handle_attach: ERROR: \"" << client + << "\" does not exist" << endl; + return; + } + if (!S_ISSOCK(stat_buf.st_mode)) { + cerr << "handle_attach: ERROR: \"" << client + << "\" exists but is not a socket" << endl; + return; + } + struct sockaddr_un *c_un = (struct sockaddr_un *)&client_addr; + int maxlen = sizeof(c_un->sun_path); + c_un->sun_family = AF_UNIX; + strncpy(c_un->sun_path, client, maxlen - 1); + c_un->sun_path[maxlen - 1] = '\0'; + client_addr_len = offsetof(struct sockaddr_un, sun_path) + + strlen(c_un->sun_path) + 1; // man 7 unix + } + + SockAddr s((struct sockaddr *)&client_addr, client_addr_len); + bool found = false; + vector::iterator it; + + pthread_mutex_lock(&clients_mutex); + + for (it = clients.begin(); it != clients.end(); it++) { + if (*it == s) { // == overloaded + found = true; + break; + } + } + + if (attach && !found) { + // attach new client + clients.push_back(s); + } else if (!attach && found) { + // detach existing client + clients.erase(it); + } + + pthread_mutex_unlock(&clients_mutex); + + send_response(src_addr, src_addr_len, "OK\n"); + +} // Commander::handle_attach + + +// Handle LIST command. +// Response to LIST is attached clients, one per line, then "OK\n". +void Commander::handle_list(const struct sockaddr *src_addr, + socklen_t src_addr_len) +{ + string response; + + pthread_mutex_lock(&clients_mutex); + + vector::iterator it; + for (it = clients.begin(); it != clients.end(); it++) + response = response + it->to_string() + "\n"; + + pthread_mutex_unlock(&clients_mutex); + + response += "OK\n"; + + send_response(src_addr, src_addr_len, response.c_str()); + +} // Commander::handle_list + + +// Handle PING command. +// Response to PING is simply "OK\n". +void Commander::handle_ping(const struct sockaddr *src_addr, + socklen_t src_addr_len) +{ + send_response(src_addr, src_addr_len, "OK\n"); +} + + +// Send a reponse to a client command. +void Commander::send_response(const struct sockaddr *src_addr, + socklen_t src_addr_len, + const char *response, ssize_t response_len) +{ + if (response_len == -1) + response_len = strlen(response); + + if (sendto(_fd, response, response_len, 0, src_addr, src_addr_len) + != response_len) { + cerr << "send_response: ERROR returned from sendto" << endl; + cerr << "send_response: \"" << strerror(errno) + << "\" sending to " << *src_addr << endl; + } else { + //cout << "send_response: OK" << endl; + } + +} // Commander::send_response + + +// print SockAddr to stream (human-readable) +ostream& operator<<(ostream& os, const SockAddr& sock) +{ + sa_family_t family = sock.su.sa.sa_family; + + if (family == AF_UNIX) { + const struct sockaddr_un *un = &sock.su.un; + os << "AF_UNIX: " << un->sun_path; + } else if (family == AF_INET) { + const struct sockaddr_in *in = &sock.su.in; + os << "AF_INET: " << inet_ntoa(in->sin_addr) << ":" + << ntohs(in->sin_port); + } else { + os << "sa_family=" << family; + } + return os; +} + + +// print sockaddr/sockaddr_un/sockaddr_in to stream (human-readable) +ostream& operator<<(ostream& os, const struct sockaddr& sa) +{ + sa_family_t family = sa.sa_family; + + if (family == AF_UNIX) { + const struct sockaddr_un *un = (const struct sockaddr_un *)&sa; + os << "AF_UNIX: " << un->sun_path; + } else if (family == AF_INET) { + const struct sockaddr_in *in = (const struct sockaddr_in *)&sa; + os << "AF_INET: " << inet_ntoa(in->sin_addr) << ":" + << ntohs(in->sin_port); + } else { + os << "sa_family=" << family; + } + return os; +} + + +#if 0 + +int main(int argc, char *argv[]) +{ + Commander *ss; + + cout << "main: creating command object" << endl; + ss = new Commander(argv[1]); + + for (int i = 0; i < 4; i++) { + ss->send_clients("hello\n", 6); + sleep(5); + } + + cout << "main: deleting command object" << endl; + delete ss; + + cout << "main: exiting" << endl; + +} // main + +#endif diff --git a/flightcode/command/Commander.h b/flightcode/command/Commander.h new file mode 100644 index 0000000..1b6f6e0 --- /dev/null +++ b/flightcode/command/Commander.h @@ -0,0 +1,128 @@ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "Log.h" + +using namespace std; + + +ostream& operator<<(ostream& os, const struct sockaddr& sa); + + +// Simple wrapper for sockaddr and its variants +class SockAddr +{ + + public: + + // Constructor mainly just checks that the supplied sockaddr fits + // (else is is probably corrupt). + SockAddr(struct sockaddr *s, socklen_t len) + { + memset(&su, 0, sizeof(su)); + sl = len; + if (sl > sizeof(su)) + sl = sizeof(su); + memcpy(&su, s, sl); + } + + // Destructor doesn't need to do anything; memory cleared for fun. + ~SockAddr() + { + memset(&su, 0, sizeof(su)); + sl = 0; + } + + // Used when adding/deleting a new SockAddr to see if a particular + // one is already in the list + const bool operator==(const SockAddr& rhs) + { + // address does not have to match after 'sl' bytes + return (sl == rhs.sl) && (memcmp(&su, &rhs.su, sl) == 0); + } + + // 'string' representation is used in the response to a 'list' + // command, where a list of all SockAddrs is being sent back. Uses + // operator<< for the conversion. + string to_string(void) + { + ostringstream ss; + ss << *this; + return ss.str(); + } + + // member accessor + const struct sockaddr *sockaddr() + { + return &su.sa; + } + + // member accessor + const socklen_t socklen() + { + return sl; + } + + friend ostream& operator<<(ostream&, const SockAddr&); + + private: + + // The actual socket address. In most cases an anonymous union would + // be better, but there are some cases where we want the maximum size. + union { + struct sockaddr sa; + struct sockaddr_in in; + struct sockaddr_un un; + } su; + + // sockaddr_un requires the length to fully specify the address + // (see 'man 7 unix', pathname vs. unnamed vs. abstract). We only + // support the pathname variant a the moment, but this is maintained + // for possible future enhancement. + socklen_t sl; + +}; // SockAddr + + +class Commander +{ + public: + Commander(const char *sock_name); + ~Commander(); + void send_clients(const void *data, int num_bytes); + + protected: + int _fd; + virtual void handle_attach(const struct sockaddr *, socklen_t, + const char *, bool); + virtual void handle_list(const struct sockaddr *, socklen_t); + virtual void handle_ping(const struct sockaddr *, socklen_t); + void send_response(const struct sockaddr *, socklen_t, + const char *, ssize_t response_len=-1); + + private: + struct sockaddr_un sa; + pthread_t thread_id; + vector clients; + pthread_mutex_t clients_mutex; + + void *command_thread(void); + + static const int MAX_CMD_LEN = 64; + + // the purpose of this is to give pthread_create a class static + // function that calls back to an object method + static void *command_entry(void *arg) + { + Commander *me = (Commander *)arg; + return me->command_thread(); + } +}; diff --git a/flightcode/command/Makefile b/flightcode/command/Makefile new file mode 100644 index 0000000..3f03b39 --- /dev/null +++ b/flightcode/command/Makefile @@ -0,0 +1,38 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +CFLAGS += -Wall -g +CXXFLAGS += -Wall -g + +LIBS = -lpthread + +SRCS_CPP = Commander.cpp +#SRCS_CPP += ../ini/cpp/INIReader.cpp ../log/Log.cpp +#SRCS_C = ../ini/ini.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = Commander + +all: $(MAIN) + +Commander.o: Commander.cpp Commander.h + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + #$(RM) ../ini/*.o + #$(RM) ../ini/cpp/*.o + #$(RM) ../log/*.o + +.PHONY: clean diff --git a/flightcode/dataflash_logger/Makefile b/flightcode/dataflash_logger/Makefile new file mode 100644 index 0000000..6bd5c43 --- /dev/null +++ b/flightcode/dataflash_logger/Makefile @@ -0,0 +1,56 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../ini ../ini/cpp + +INCS = -I../util -I../ini -I../ini/cpp + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) -std=c++11 + +SRCS_CPP = dataflash_logger.cpp +SRCS_CPP += common_tool.cpp +SRCS_CPP += format_reader.cpp +SRCS_CPP += telem_forwarder_client.cpp +SRCS_CPP += mavlink_reader.cpp +SRCS_CPP += analyzer_util.cpp +SRCS_CPP += dataflash_logger_program.cpp +SRCS_CPP += INIReader.cpp +SRCS_CPP += mavlink_message_handler.cpp +SRCS_CPP += mavlink_writer.cpp +SRCS_CPP += telem_client.cpp +SRCS_CPP += telem_serial.cpp +SRCS_CPP += la-log.cpp +SRCS_CPP += heart.cpp +SRCS_C = util.c ini.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) +LIBS=-pthread + +MAIN = dataflash_logger # actually, the main is in mavlink_reader... + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: clean diff --git a/flightcode/dataflash_logger/MsgHandler.cpp b/flightcode/dataflash_logger/MsgHandler.cpp new file mode 100644 index 0000000..17628a4 --- /dev/null +++ b/flightcode/dataflash_logger/MsgHandler.cpp @@ -0,0 +1,264 @@ +#include "MsgHandler.h" + +#include + +void fatal(const char *msg) +{ + ::printf("%s", msg); + ::printf("\n"); + exit(1); +} + +char *xstrdup(const char *string) +{ + char *ret = strdup(string); + if (ret == NULL) { + perror("strdup"); + fatal("strdup failed"); + } + return ret; +} + +void MsgHandler::add_field_type(char type, size_t size) +{ + size_for_type_table[(type > 'A' ? (type - 'A') : (type - 'a'))] = size; +} + +uint8_t MsgHandler::size_for_type(char type) +{ + return size_for_type_table[(uint8_t)(type > 'A' ? (type - 'A') : (type - 'a'))]; +} + +void MsgHandler::init_field_types() +{ + add_field_type('b', sizeof(int8_t)); + add_field_type('c', sizeof(int16_t)); + add_field_type('e', sizeof(int32_t)); + add_field_type('f', sizeof(float)); + add_field_type('h', sizeof(int16_t)); + add_field_type('i', sizeof(int32_t)); + add_field_type('n', sizeof(char[4])); + add_field_type('B', sizeof(uint8_t)); + add_field_type('C', sizeof(uint16_t)); + add_field_type('E', sizeof(uint32_t)); + add_field_type('H', sizeof(uint16_t)); + add_field_type('I', sizeof(uint32_t)); + add_field_type('L', sizeof(int32_t)); + add_field_type('M', sizeof(uint8_t)); + add_field_type('N', sizeof(char[16])); + add_field_type('Z', sizeof(char[64])); + add_field_type('q', sizeof(int64_t)); + add_field_type('Q', sizeof(uint64_t)); +} + +struct MsgHandler::format_field_info *MsgHandler::find_field_info(const char *label) +{ + for (uint8_t i = 0; i < next_field; i++) { + if (streq(field_info[i].label, label)) { + return &field_info[i]; + } + } + return NULL; +} + +MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f) +{ + init_field_types(); + parse_format_fields(); +} + +void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset, uint8_t _length) +{ + field_info[next_field].label = xstrdup(_label); + field_info[next_field].type = _type; + field_info[next_field].offset = _offset; + field_info[next_field].length = _length; + next_field++; +} + +void MsgHandler::parse_format_fields() +{ + char *labels = xstrdup(f.labels); + char *arg = labels; + uint8_t label_offset = 0; + char *next_label; + uint8_t msg_offset = 3; // 3 bytes for the header + + while ((next_label = strtok(arg, ",")) != NULL) { + if (label_offset > strlen(f.format)) { + free(labels); + printf("too few field times for labels %s (format=%s) (labels=%s)\n", f.name, f.format, + f.labels); + exit(1); + } + uint8_t field_type = f.format[label_offset]; + uint8_t length = size_for_type(field_type); + add_field(next_label, field_type, msg_offset, length); + arg = NULL; + msg_offset += length; + label_offset++; + } + + if (label_offset != strlen(f.format)) { + printf("too few labels for format (format=%s) (labels=%s)\n", f.format, f.labels); + } + + free(labels); +} + +bool MsgHandler::field_value(const uint8_t *msg, const char *label, char *ret, uint8_t retlen) +{ + struct format_field_info *info = find_field_info(label); + if (info == NULL) { + ::printf("No info for (%s)\n", label); + abort(); + } + + uint8_t offset = info->offset; + if (offset == 0) { + return false; + } + + memset(ret, '\0', retlen); + + memcpy(ret, &msg[offset], (retlen < info->length) ? retlen : info->length); + + return true; +} + +bool MsgHandler::field_value(uint8_t *msg, const char *label, Vector3f &ret) +{ + const char *axes = "XYZ"; + uint8_t i; + for (i = 0; i < next_field; i++) { + if (!strncmp(field_info[i].label, label, strlen(label)) && + strlen(field_info[i].label) == strlen(label) + 1) { + for (uint8_t j = 0; j < 3; j++) { + if (field_info[i].label[strlen(label)] == axes[j]) { + field_value_for_type_at_offset(msg, field_info[i].type, field_info[i].offset, + ret[j]); + break; // break from finding-label loop + } + } + } + if (i == next_field) { + return 0; // not found + } + } + + return true; +} + +void MsgHandler::string_for_labels(char *buffer, uint bufferlen) +{ + memset(buffer, '\0', bufferlen); + bufferlen--; + + char *pos = buffer; + for (uint8_t k = 0; k < LOGREADER_MAX_FIELDS; k++) { + if (field_info[k].label != NULL) { + uint8_t remaining = bufferlen - (pos - buffer); + uint8_t label_length = strlen(field_info[k].label); + uint8_t required = label_length; + if (pos != buffer) { // room for a comma + required++; + } + if (required + 1 > remaining) { // null termination + break; + } + + if (pos != buffer) { + *pos++ = ','; + } + + memcpy(pos, field_info[k].label, label_length); + pos += label_length; + } + } +} + +MsgHandler::~MsgHandler() +{ + for (uint8_t k = 0; k < LOGREADER_MAX_FIELDS; k++) { + if (field_info[k].label != NULL) { + free(field_info[k].label); + } + } +} + +void MsgHandler::location_from_msg(uint8_t *msg, Location &loc, const char *label_lat, + const char *label_long, const char *label_alt) +{ + loc.lat = require_field_int32_t(msg, label_lat); + loc.lng = require_field_int32_t(msg, label_long); + loc.alt = require_field_int32_t(msg, label_alt); + loc.options = 0; +} + +void MsgHandler::ground_vel_from_msg(uint8_t *msg, Vector3f &vel, const char *label_speed, + const char *label_course, const char *label_vz) +{ + uint32_t ground_speed; + int32_t ground_course; + require_field(msg, label_speed, ground_speed); + require_field(msg, label_course, ground_course); + vel[0] = ground_speed * 0.01f * cosf(radians(ground_course * 0.01f)); + vel[1] = ground_speed * 0.01f * sinf(radians(ground_course * 0.01f)); + vel[2] = require_field_float(msg, label_vz); +} + +void MsgHandler::attitude_from_msg(uint8_t *msg, Vector3f &att, const char *label_roll, + const char *label_pitch, const char *label_yaw) +{ + att[0] = require_field_int16_t(msg, label_roll) * 0.01f; + att[1] = require_field_int16_t(msg, label_pitch) * 0.01f; + att[2] = require_field_uint16_t(msg, label_yaw) * 0.01f; +} + +void MsgHandler::field_not_found(const uint8_t *msg, const char *label) +{ + char all_labels[256]; + uint8_t type = msg[2]; + string_for_labels(all_labels, 256); + ::printf("Field (%s) not found for id=%d; options are (%s)\n", label, type, all_labels); + abort(); +} + +void MsgHandler::require_field(const uint8_t *msg, const char *label, char *buffer, + uint8_t bufferlen) +{ + if (!field_value(msg, label, buffer, bufferlen)) { + field_not_found(msg, label); + } +} + +float MsgHandler::require_field_float(const uint8_t *msg, const char *label) +{ + float ret; + require_field(msg, label, ret); + return ret; +} +uint8_t MsgHandler::require_field_uint8_t(const uint8_t *msg, const char *label) +{ + uint8_t ret; + require_field(msg, label, ret); + return ret; +} +int32_t MsgHandler::require_field_int32_t(const uint8_t *msg, const char *label) +{ + int32_t ret; + require_field(msg, label, ret); + return ret; +} +uint16_t MsgHandler::require_field_uint16_t(const uint8_t *msg, const char *label) +{ + uint16_t ret; + require_field(msg, label, ret); + return ret; +} +int16_t MsgHandler::require_field_int16_t(const uint8_t *msg, const char *label) +{ + int16_t ret; + require_field(msg, label, ret); + return ret; +} diff --git a/flightcode/dataflash_logger/MsgHandler.h b/flightcode/dataflash_logger/MsgHandler.h new file mode 100644 index 0000000..63d4a05 --- /dev/null +++ b/flightcode/dataflash_logger/MsgHandler.h @@ -0,0 +1,152 @@ +#ifndef AP_MSGHANDLER_H +#define AP_MSGHANDLER_H + +#include +#include +#include // for abort() + +#include + +#define radians(x) (x / 180 * M_PI) +#include "Vector3f.h" +#include "Location.h" +#include "DataFlash/LogMessage.h" + +#define LOGREADER_MAX_FIELDS 30 + +#ifndef streq +#define streq(x, y) (!strcmp(x, y)) +#endif + +class MsgHandler +{ +public: + // constructor - create a parser for a MavLink message format + MsgHandler(const struct log_Format &f); + + // retrieve a comma-separated list of all labels + void string_for_labels(char *buffer, uint bufferlen); + + // field_value - retrieve the value of a field from the supplied message + // these return false if the field was not found + template < typename R > + bool field_value(const uint8_t *msg, const char *label, R &ret); + + bool field_value(uint8_t *msg, const char *label, Vector3f &ret); + bool field_value(const uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen); + + template < typename R > + void require_field(const uint8_t *msg, const char *label, R &ret) + { + if (!field_value(msg, label, ret)) { + field_not_found(msg, label); + } + } + void require_field(const uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen); + float require_field_float(const uint8_t *msg, const char *label); + uint8_t require_field_uint8_t(const uint8_t *msg, const char *label); + int32_t require_field_int32_t(const uint8_t *msg, const char *label); + uint16_t require_field_uint16_t(const uint8_t *msg, const char *label); + int16_t require_field_int16_t(const uint8_t *msg, const char *label); + +private: + void add_field(const char *_label, uint8_t _type, uint8_t _offset, uint8_t length); + + template < typename R > + void field_value_for_type_at_offset(const uint8_t *msg, uint8_t type, uint8_t offset, R &ret); + + struct format_field_info { // parsed field information + char *label; + uint8_t type; + uint8_t offset; + uint8_t length; + }; + struct format_field_info field_info[LOGREADER_MAX_FIELDS] = {}; + + uint8_t next_field; + size_t size_for_type_table[52]; // maps field type (e.g. 'f') to e.g 4 bytes + + void parse_format_fields(); + void init_field_types(); + void add_field_type(char type, size_t size); + uint8_t size_for_type(char type); + +protected: + struct format_field_info *find_field_info(const char *label); + + struct log_Format f; // the format we are a parser for + ~MsgHandler(); + + void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat, + const char *label_long, const char *label_alt); + + void ground_vel_from_msg(uint8_t *msg, Vector3f &vel, const char *label_speed, + const char *label_course, const char *label_vz); + + void attitude_from_msg(uint8_t *msg, Vector3f &att, const char *label_roll, + const char *label_pitch, const char *label_yaw); + void field_not_found(const uint8_t *msg, const char *label); +}; + +template < typename R > +bool MsgHandler::field_value(const uint8_t *msg, const char *label, R &ret) +{ + struct format_field_info *info = find_field_info(label); + if (info == NULL) { + return false; + } + + uint8_t offset = info->offset; + if (offset == 0) { + return false; + } + + field_value_for_type_at_offset(msg, info->type, offset, ret); + + return true; +} + +template < typename R > +inline void MsgHandler::field_value_for_type_at_offset(const uint8_t *msg, uint8_t type, + uint8_t offset, R &ret) +{ + /* we register the types - add_field_type - so can we do without + * this switch statement somehow? */ + switch (type) { + case 'B': + ret = (R)(((uint8_t *)&msg[offset])[0]); + break; + case 'c': + case 'h': + ret = (R)(((int16_t *)&msg[offset])[0]); + break; + case 'H': + ret = (R)(((uint16_t *)&msg[offset])[0]); + break; + case 'C': + ret = (R)(((uint16_t *)&msg[offset])[0]); + break; + case 'f': + ret = (R)(((float *)&msg[offset])[0]); + break; + case 'I': + case 'E': + ret = (R)(((uint32_t *)&msg[offset])[0]); + break; + case 'L': + case 'e': + ret = (R)(((int32_t *)&msg[offset])[0]); + break; + case 'q': + ret = (R)(((int64_t *)&msg[offset])[0]); + break; + case 'Q': + ret = (R)(((uint64_t *)&msg[offset])[0]); + break; + default: + ::printf("Unhandled format type (%c)\n", type); + ::abort(); + } +} + +#endif diff --git a/flightcode/dataflash_logger/analyzer_util.cpp b/flightcode/dataflash_logger/analyzer_util.cpp new file mode 100644 index 0000000..fabdb9b --- /dev/null +++ b/flightcode/dataflash_logger/analyzer_util.cpp @@ -0,0 +1,76 @@ +#include "analyzer_util.h" +#include + +#include + +void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T) +{ + struct tm *tmp; + time_t t = T / 1000000; + tmp = localtime(&t); + ::strftime(buf, buflen, "%Y%m%d%H%M%S", tmp); +} + +uint64_t now() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * (uint64_t)1000000 + tv.tv_usec; +} + +double vec_len(double vec[3]) +{ + return sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); +} + +double earthradius() // in metres +{ + return 6371000.0f; +} + +double wrap_valid_longitude(const double longitude) +{ + if (longitude < 180.0f) { + return longitude; + } + + return 180 - (longitude - 180); + // return (((longitude + 180.0) % 360.0) -180.0); +} + +// http://www.movable-type.co.uk/scripts/latlong.html +void gps_newpos(const double orig_lat, const double orig_lon, const double bearing, + const double distance, double &dest_lat, double &dest_lon) +{ + double origin_lat_rad = deg_to_rad(orig_lat); + double origin_lon_rad = deg_to_rad(orig_lon); + double bearing_rad = deg_to_rad(bearing); + + double dr = distance / earthradius(); + + dest_lat = + asin(sin(origin_lat_rad) * cos(dr) + cos(origin_lat_rad) * sin(dr) * cos(bearing_rad)); + dest_lon = origin_lon_rad + atan2(sin(bearing_rad) * sin(dr) * cos(origin_lat_rad), + cos(dr) - sin(origin_lat_rad) * sin(origin_lat_rad)); + dest_lat = rad_to_deg(dest_lat); + dest_lon = wrap_valid_longitude(rad_to_deg(dest_lon)); +} + +// origin_lat in degrees +// origin_lon in degrees +// bearing in degrees +// distance in metres +void gps_offset(double orig_lat, double orig_lon, double east, double north, double &dest_lat, + double &dest_lon) +{ + double bearing = rad_to_deg(atan2(east, north)); + double distance = sqrt(east * east + north * north); + gps_newpos(orig_lat, orig_lon, bearing, distance, dest_lat, dest_lon); +} + +double altitude_from_pressure_delta(double gnd_abs_press, double gnd_temp, double press_abs, + double temp UNUSED) +{ + double scaling = press_abs / gnd_abs_press; + return 153.8462 * (gnd_temp + 273.15) * (1.0 - exp(0.190259 * log(scaling))); +} diff --git a/flightcode/dataflash_logger/analyzer_util.h b/flightcode/dataflash_logger/analyzer_util.h new file mode 100644 index 0000000..2832653 --- /dev/null +++ b/flightcode/dataflash_logger/analyzer_util.h @@ -0,0 +1,75 @@ +#ifndef _ANALYZER_UTIL +#define _ANALYZER_UTIL + +#include +#include + +// from: http://stackoverflow.com/questions/2342162/stdstring-formatting-like-sprintf +#include + +double earthradius(); + +double wrap_valid_longitude(const double longitude); + +// http://www.movable-type.co.uk/scripts/latlong.html +void gps_newpos(const double orig_lat, const double orig_lon, const double bearing, + const double distance, double &dest_lat, double &dest_lon); + +// origin_lat in degrees +// origin_lon in degrees +// bearing in degrees +// distance in metres +void gps_offset(double orig_lat, double orig_lon, double east, double north, double &dest_lat, + double &dest_lon); + +double altitude_from_pressure_delta(double gnd_abs_press, double gnd_temp, double press_abs, + double temp); + +template < typename... Args > +std::string string_format(const char *format, const Args... args) +{ + + int32_t size = snprintf(nullptr, 0, format, args...); + if (size < 0) { + ::fprintf(stderr, "snprintf error (%d): %s\n", size, strerror(errno)); + abort(); + } + size += 1; // Extra space for '\0' + std::unique_ptr< char[] > buf(new char[size]); + snprintf(buf.get(), size, format, args...); + return std::string(buf.get(), buf.get() + size - 1); // We don't want the '\0' inside +} + +template < typename... Args > +std::string string_format(const std::string format, const Args... args) +{ + return string_format(format.c_str(), args...); +} + +#ifndef streq +#define streq(x, y) (!strcmp(x, y)) +#endif + +// inline float deg_to_rad(const float deg) { +// return deg/M_PI * 180; +// } + +// inline float rad_to_deg(const float rad) { +// return rad*180/M_PI; +// } + +#define deg_to_rad(x) ((x)*M_PI / 180.0f) +#define rad_to_deg(x) ((x)*180.0f / M_PI) + +#define is_zero(x) (x < 0.00001) +#define is_equal(x, y) (is_zero(fabs((x) - (y)))) + +void format_timestamp(char *buf, uint8_t buflen, uint64_t T); + +uint64_t now(); + +double vec_len(double vec[3]); + +#define UNUSED __attribute__((unused)) + +#endif // _ANALYZER_UTIL diff --git a/flightcode/dataflash_logger/analyzervehicle_copter.cpp b/flightcode/dataflash_logger/analyzervehicle_copter.cpp new file mode 100644 index 0000000..eed23fb --- /dev/null +++ b/flightcode/dataflash_logger/analyzervehicle_copter.cpp @@ -0,0 +1,127 @@ +#include "analyzervehicle_copter.h" + +using namespace AnalyzerVehicle; + +bool Copter::param_default(const char *name, float &ret) +{ + if (_frame_type == frame_type_quad) { + if (_param_defaults_quad[name]) { + ret = _param_defaults_quad[name]; + return true; + } + } + if (_param_defaults[name]) { + ret = _param_defaults[name]; + return true; + } + return Base::param_default(name, ret); +} + +/* I think there's an argument for moving the following into Analyzer: */ + +bool Copter::is_flying() +{ + if (!is_armed()) { + // we hope we're not flying, anyway! + return false; + } + + if (!any_motor_running_fast()) { + return false; + } + + return true; +} + +bool Copter::any_motor_running_fast() +{ + for (uint8_t i = 1; i < _num_motors; i++) { + if (_servo_output[i] > is_flying_motor_threshold) { + return true; + } + } + return false; +} + +std::set< uint8_t > Copter::motors_clipping_high() +{ + std::set< uint8_t > ret; + char label[] = "RCx_MAX"; + for (uint8_t i = 1; i <= _num_motors; i++) { + label[2] = '0' + i; + float max; + if (param(label, max)) { + uint16_t delta = abs((int32_t)_servo_output[i] - (uint16_t)max); + if ((float)delta / max < .05) { // within 5% + ret.insert(i); + } + } + } + return ret; +} + +std::set< uint8_t > Copter::motors_clipping_low() +{ + std::set< uint8_t > ret; + char label[] = "RCx_MIN"; + for (uint8_t i = 1; i <= _num_motors; i++) { + label[2] = '0' + i; + float min; + if (param(label, min)) { + if (_servo_output[i] < (uint16_t)min || + _servo_output[i] - min < 105) { // FIXME: constant + ret.insert(i); + } + // uint16_t delta = abs((int32_t)_servo_output[i] - (uint16_t)min); + // ::fprintf(stderr, "%d delta=%d (%f)\n", i, delta, (float)delta/min); + // if ((float)delta/min < .05) { + // ::fprintf(stderr, "%d clipping low \n", i); + // ret.insert(i); + // } + } + } + return ret; +} + +void Copter::set_frame_type(copter_frame_type frame_type) +{ + _frame_type = frame_type; + switch (frame_type) { + case frame_type_quad: + _num_motors = 4; + break; + case frame_type_y6: + _num_motors = 6; + break; + case invalid: + ::fprintf(stderr, "Invalid frame type"); + abort(); + } +} + +bool Copter::exceeding_angle_max() +{ + float angle_max; // convert from centidegrees + if (param_with_defaults("ANGLE_MAX", angle_max)) { + angle_max /= 100; + if (fabs(att().roll()) > angle_max) { + return true; + } + if (fabs(att().pitch()) > angle_max) { + return true; + } + } + return false; +} + +void Copter::set_frame(const char *frame_config_string) +{ + if (strstr(frame_config_string, "QUAD")) { + set_frame_type(AnalyzerVehicle::Copter::frame_type_quad); + } else if (strstr(frame_config_string, "Y6")) { + set_frame_type(AnalyzerVehicle::Copter::frame_type_y6); + } else { + ::fprintf(stderr, "Unknown frame (%s)\n", frame_config_string); + abort(); + } +} diff --git a/flightcode/dataflash_logger/common_tool.cpp b/flightcode/dataflash_logger/common_tool.cpp new file mode 100644 index 0000000..e16f911 --- /dev/null +++ b/flightcode/dataflash_logger/common_tool.cpp @@ -0,0 +1,173 @@ +#include "common_tool.h" + +#include +#include +#include + +#include "la-log.h" + +void Common_Tool::init_config() +{ + if (config_filename == NULL) { + abort(); + } + struct stat buf; + if (stat(config_filename, &buf) == -1) { + if (errno == ENOENT) { + if (!streq(default_config_filename, config_filename)) { + la_log(LOG_CRIT, "Config file (%s) does not exist", config_filename); + exit(1); + } + } else { + la_log(LOG_CRIT, "Failed to stat (%s): %s\n", config_filename, strerror(errno)); + exit(1); + } + } + _config = new INIReader(config_filename); + if (_config == NULL) { + la_log(LOG_CRIT, "Failed to create config from (%s)\n", config_filename); + exit(1); + } + if (_config == NULL) { + _config = new INIReader("/dev/null"); + } +} + +void Common_Tool::check_fds_are_empty_after_select(fd_set &fds_read, fd_set &fds_write, + fd_set &fds_err, uint8_t nfds) +{ + for (uint8_t i = 0; i < nfds; i++) { + if (FD_ISSET(i, &fds_read)) { + la_log(LOG_ERR, "fds_read not empty"); + break; + } + if (FD_ISSET(i, &fds_write)) { + la_log(LOG_ERR, "fds_write not empty"); + break; + } + if (FD_ISSET(i, &fds_err)) { + la_log(LOG_ERR, "fds_err not empty"); + break; + } + } +} + +void Common_Tool::pack_select_fds(fd_set &fds_read UNUSED, fd_set &fds_write UNUSED, + fd_set &fds_err UNUSED, uint8_t &nfds UNUSED) +{ +} + +void Common_Tool::handle_select_fds(fd_set &fds_read UNUSED, fd_set &fds_write UNUSED, + fd_set &fds_err UNUSED, uint8_t &nfds UNUSED) +{ +} + +void Common_Tool::sighup_received_tophalf() +{ +} +void Common_Tool::sighup_handler(int signal UNUSED) +{ + _sighup_received = true; +} +void Common_Tool::do_idle_callbacks() +{ +} + +uint32_t Common_Tool::select_timeout_us() +{ + return 200000; +} + +void Common_Tool::select_loop() +{ + fd_set fds_read; + fd_set fds_write; + fd_set fds_err; + uint8_t nfds; + while (1) { + if (_sighup_received) { + sighup_received_tophalf(); + _sighup_received = false; + } + /* Wait for a packet, or time out if no packets arrive so we always + periodically log status and check for new destinations. Downlink + packets are on the order of 100/sec, so the timeout is such that + we don't expect timeouts unless solo stops sending packets. We + almost always get a packet with a 200 msec timeout, but not with + a 100 msec timeout. (Timeouts don't really matter though.) */ + + struct timeval timeout; + + FD_ZERO(&fds_read); + FD_ZERO(&fds_write); + FD_ZERO(&fds_err); + nfds = 0; + pack_select_fds(fds_read, fds_write, fds_err, nfds); + + timeout.tv_sec = 0; + timeout.tv_usec = select_timeout_us(); + int res = select(nfds, &fds_read, &fds_write, &fds_err, &timeout); + + if (res < 0) { + unsigned skipped = 0; + // if ((skipped = can_log_error()) >= 0) + la_log(LOG_ERR, "[%u] select: %s", skipped, strerror(errno)); + /* this sleep is to avoid soaking the CPU if select starts + returning immediately for some reason */ + /* previous code was not checking errfds; we are now, so + perhaps this usleep can go away -pb20150730 */ + usleep(10000); + continue; + } + + if (res == 0) { + // select timeout + } + + handle_select_fds(fds_read, fds_write, fds_err, nfds); + + check_fds_are_empty_after_select(fds_read, fds_write, fds_err, nfds); + + do_idle_callbacks(); + } /* while (1) */ +} + +void Common_Tool::parse_fd(Format_Reader *reader, int fd) +{ + char buf[1 << 16]; + ssize_t buf_start = 0; + while (true) { + ssize_t bytes_read = read(fd, &buf[buf_start], sizeof(buf) - buf_start); + if (bytes_read == -1) { + fprintf(stderr, "Read failed: %s\n", strerror(errno)); + exit(1); + } + if (bytes_read == 0) { + while (reader->feed((uint8_t *)buf, buf_start + bytes_read)) { + } + break; + } + + bytes_read += buf_start; + ssize_t total_bytes_used = 0; + while (total_bytes_used < bytes_read) { + ssize_t bytes_used = + reader->feed((uint8_t *)(&buf[total_bytes_used]), bytes_read - total_bytes_used); + if (bytes_used > bytes_read - total_bytes_used) { + abort(); + } + if (bytes_used == 0) { + break; + } + total_bytes_used += bytes_used; + } + // ::fprintf(stderr, "total_bytes_used = %u\n", total_bytes_used); + // ::fprintf(stderr, "bytes_read = %u\n", bytes_read); + memcpy(&buf[0], (uint8_t *)(&(buf[total_bytes_used])), bytes_read - total_bytes_used); + buf_start = bytes_read - total_bytes_used; + } + + reader->end_of_log(); + + // ::fprintf(stderr, "Packet count: %d\n", packet_count); +} diff --git a/flightcode/dataflash_logger/common_tool.h b/flightcode/dataflash_logger/common_tool.h new file mode 100644 index 0000000..7bf9476 --- /dev/null +++ b/flightcode/dataflash_logger/common_tool.h @@ -0,0 +1,44 @@ +#include "util.h" +#include "analyzer_util.h" +#include "INIReader.h" + +#include "format_reader.h" + +class Common_Tool +{ +public: + Common_Tool() : config_filename(default_config_filename) + { + } + + void sighup_handler(int signal); + + void parse_fd(Format_Reader *reader, int fd); + +protected: + class INIReader *config() + { + return _config; + }; + void init_config(); + const char *default_config_filename = "/etc/sololink.conf"; + const char *config_filename; + + virtual void sighup_received_tophalf(); + + bool _sighup_received = false; // FIXME: scope + + virtual uint32_t select_timeout_us(); + void select_loop(); + + virtual void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds); + virtual void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds); + virtual void do_idle_callbacks(); + +private: + void check_fds_are_empty_after_select(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t nfds); + class INIReader *_config = NULL; +}; diff --git a/flightcode/dataflash_logger/dataflash_logger.cpp b/flightcode/dataflash_logger/dataflash_logger.cpp new file mode 100644 index 0000000..b6a08fa --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_logger.cpp @@ -0,0 +1,477 @@ +#include "dataflash_logger.h" + +#include +#include // for snprintf +#include +#include +#include + +#include +#include +#include // for sched_yield + +#include "la-log.h" +#include "util.h" +#include "../mavlink/c_library/common/mavlink.h" + +/* + +There are currently two threads: a "network" thread (see handle_message), and the fileio thread (see +fileio_task). The network thread takes the data from the packets and packs it into _write_buffers. +It then passes the buffers to the fileio thread for writing to disk. The buffers are then passed +back to the network thread. + +Notes on thread safety: + +la_log is called by both the network and IO threads. openlog/syslog are thread safe. The la_log +code is not designed to be thread safe. That code is commented as such, and in future locking may +be required there. The current implementation has races concerning message suppression but fixing +these may lead to more complexity than is justified. + +The fileio and network threads in dataflash logger share: + - out_fd + - _write_buffers[] + +out_fd is not to be touched in the main thread after it has been opened - unless logging has +stopped, and all the buffers are owned by the network thread. The fileio thread may only touch +out_fd and _write_buffers if logging has started, or it owns buffers. + +The buffers contained in _write_buffers are owned by one thread at a time. Only the owner may +modify the buffer. Ownership is transfered by assignment of the owner variable. This will work for +two threads, but locking of this transfer will be required if more threads become involved, and the +programmer is suitably paranoid. + +*/ + +void *fileio_task_wrapper(void *instance) +{ + ((DataFlash_Logger *)instance)->fileio_task(); + return NULL; +} + +DataFlash_Logger::DataFlash_Logger(MAVLink_Writer *mavlink_writer) + : MAVLink_Message_Handler(), _mavlink_writer(mavlink_writer), + target_system_id(target_system_id_default), target_component_id(target_component_id_default), + seqno_gap_nack_threshold(20) +{ + _current_write_buffer = &_write_buffers[0]; + pthread_mutex_init(&_write_buffer_mutex, NULL); + pthread_cond_init(&_wakeup_fileio_thread, NULL); + pthread_create(&_fileio_thread, NULL, fileio_task_wrapper, this); +} + +void DataFlash_Logger::write_buffer_to_disk(const struct df_write_buffer *buffer) +{ + const uint8_t length = MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN; + for (uint16_t j = 0; j < buffer->next_defered_write; j++) { + const struct defered_write *some_write = &buffer->defered_writes[j]; + const uint32_t seqno = some_write->seqno; + const uint8_t *data = some_write->data; + if (lseek(out_fd, seqno * length, SEEK_SET) == -1) { + la_log(LOG_ERR, "mh-dfl: lseek (%u) failed: %s", seqno * length, strerror(errno)); + return; + } + ssize_t written = write(out_fd, data, length); + if (written < length) { + la_log(LOG_ERR, "mh-dfl: short write: %s", strerror(errno)); + return; + } + } + + // TODO: write all of the blocks in the buffer to disk, trying to + // coalesce writes as we go: +} + +void DataFlash_Logger::fileio_task() +{ + while (true) { + // we should never be called while fd == -1. + pthread_cond_wait(&_wakeup_fileio_thread, &_write_buffer_mutex); + // we don't actually need the mutex: + pthread_mutex_unlock(&_write_buffer_mutex); + + df_write_buffer *buffer = next_write_buffer(FILEIO_THREAD); + while (buffer != NULL) { + write_buffer_to_disk(buffer); + buffer->next_defered_write = 0; + // hand the buffer back to the net thread: + buffer->owner = NETWORK_THREAD; + buffer = next_write_buffer(FILEIO_THREAD); + fsync(out_fd); + + uint64_t now = clock_gettime_us(CLOCK_MONOTONIC); + if (now - fileio_last_log_time > 10000000) { + // the following isn't quite right given we seek around... + la_log(LOG_INFO, "mh-dfl: Current log size: %lu", lseek(out_fd, 0, SEEK_CUR)); + fileio_last_log_time = now; + } + } + } +} + +void DataFlash_Logger::sighup_received() +{ + logging_stop(); +} + +void DataFlash_Logger::idle_tenthHz() +{ +} + +void DataFlash_Logger::idle_1Hz() +{ + if (!logging_started) { + if (sender_system_id != 0) { + // we've previously been logging, so telling the other end + // to stop logging may let us restart logging sooner + send_stop_logging_packet(); + } + send_start_logging_packet(); + } +} +void DataFlash_Logger::idle_10Hz() +{ + if (logging_started) { + // if no data packet in 10 seconds then close log + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (now_us - _last_data_packet_time > 10000000) { + la_log(LOG_INFO, "mh-dfl: No data packets received for some time (now=%llu last=%llu). " + " Closing log. Final log size: %lu", + now_us, _last_data_packet_time, lseek(out_fd, 0, SEEK_CUR)); + logging_stop(); + } + } +} + +void DataFlash_Logger::idle_100Hz() +{ + push_response_queue(); +} + +void DataFlash_Logger::send_response(uint32_t seqno, bool status) +{ + mavlink_message_t msg; + mavlink_msg_remote_log_block_status_pack(system_id, component_id, &msg, sender_system_id, + sender_component_id, seqno, status); + _mavlink_writer->send_message(msg); +} + +void DataFlash_Logger::push_response_queue() +{ + const uint8_t max_packets_to_send = 5; + uint8_t packets_sent = 0; + while (response_queue_head != response_queue_tail && packets_sent < max_packets_to_send) { + send_response(responses[response_queue_tail].seqno, responses[response_queue_tail].status); + response_queue_tail++; + if (response_queue_tail >= RESPONSE_QUEUE_LENGTH) { + response_queue_tail = 0; + } + } +} + +bool DataFlash_Logger::configure(INIReader *config) +{ + if (!MAVLink_Message_Handler::configure(config)) { + return false; + } + + std::string path = config->Get("dflogger", "log_dirpath", "/log/dataflash"); + _log_directory_path = strdup(path.c_str()); + if (_log_directory_path == NULL) { + return false; + } + + // need to do equivalent of 'mkdir -p log_dirpath' here + // (i.e. handle multi-level path) + if (mkdir(_log_directory_path, + S_IRUSR | S_IWUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH) != 0) { + // this is expected to succeed on the first boot after a factory or + // settings reset, then fail with EEXIST each startup after that. + if (errno != EEXIST) { + la_log(LOG_ERR, "mh-dfl: Failed to create (%s): %s", _log_directory_path, + strerror(errno)); + return false; + } + } + + target_system_id = config->GetInteger("dflogger", "target_system_id", 0); + target_component_id = config->GetInteger("dflogger", "target_component_id", 0); + + return true; +} + +bool DataFlash_Logger::make_new_log_filename(char *buffer, uint8_t bufferlen) +{ + uint8_t lastlog_buflen = 128; + char lastlog_buf[128]; + // this was really beautiful, but I don't think SoloLink has an + // RTC; it depends on GPS to update its clock (scribbled down + // periodically?) + + // time_t t; + + // time(&t); + // struct tm *timebits = gmtime(&t); + + // snprintf(buffer, bufferlen, "%s/%04d%02d%02d%02d%02d%02d.BIN", + // _log_directory_path, + // timebits->tm_year+1900, + // timebits->tm_mon+1, + // timebits->tm_mday, + // timebits->tm_hour, + // timebits->tm_min, + // timebits->tm_sec); + + memset(lastlog_buf, '\0', lastlog_buflen); + snprintf(lastlog_buf, lastlog_buflen, "%s/LASTLOG.TXT", _log_directory_path); + int fd; + uint32_t num; + if ((fd = open(lastlog_buf, O_RDONLY)) == -1) { + if (errno != ENOENT) { + // what? + syslog(LOG_ERR, "Failed to open (%s) for reading: %s", lastlog_buf, strerror(errno)); + return false; + } + num = 1; + } else { + uint8_t numbuf_len = 128; + char numbuf[numbuf_len]; + memset(numbuf, '\0', numbuf_len); + int bytes_read = read(fd, numbuf, numbuf_len); + close(fd); + if (bytes_read == -1) { + return false; + } + num = strtoul(numbuf, NULL, 10); + num++; + } + + if ((fd = open(lastlog_buf, O_WRONLY | O_TRUNC | O_CREAT)) == -1) { + // *shrug* We will continue to overwrite, I guess... + } else { + const uint8_t outsize = 16; + char out[outsize]; + memset(out, '\0', outsize); + int towrite = snprintf(out, outsize, "%d\r\n", num); + write(fd, out, towrite); // ignore return... + close(fd); + } + + snprintf(buffer, bufferlen, "%s/%d.BIN", _log_directory_path, num); + + return true; +} + +bool DataFlash_Logger::output_file_open() +{ + const uint8_t filename_length = 64; + char filename[filename_length]; + + if (!make_new_log_filename(filename, filename_length)) { + return false; + } + + out_fd = open(filename, O_WRONLY | O_CREAT | O_TRUNC, 0777); + if (out_fd == -1) { + printf("Failed to open (%s): %s\n", filename, strerror(errno)); + la_log(LOG_ERR, "mh-dfl: Failed to open (%s): %s", filename, strerror(errno)); + return false; + } + la_log(LOG_INFO, "mh-dfl: Opened log file (%s)", filename); + + return true; +} + +void DataFlash_Logger::output_file_close() +{ + close(out_fd); + out_fd = -1; +} + +void DataFlash_Logger::queue_response(uint32_t seqno, bool status) +{ + responses[response_queue_head].seqno = seqno; + responses[response_queue_head].status = status; + response_queue_head++; + if (response_queue_head >= RESPONSE_QUEUE_LENGTH) { + response_queue_head = 0; + } +} + +void DataFlash_Logger::queue_ack(uint32_t seqno) +{ + queue_response(seqno, true); +} + +void DataFlash_Logger::queue_nack(uint32_t seqno) +{ + queue_response(seqno, false); +} + +void DataFlash_Logger::queue_gap_nacks(uint32_t seqno) +{ + if (seqno <= highest_seqno_seen) { + // this packet filled in a gap (or was a dupe) + return; + } + + if (seqno - highest_seqno_seen > seqno_gap_nack_threshold) { + // we've seen some serious disruption, and lots of stuff + // is probably going to be lost. Do not bother NACKing + // packets here and let the server sort things out + return; + } + + for (uint32_t i = highest_seqno_seen + 1; i < seqno; i++) { + queue_nack(i); + } +} + +bool DataFlash_Logger::logging_start(mavlink_remote_log_data_block_t &msg UNUSED) +{ + sender_system_id = most_recent_sender_system_id; + sender_component_id = most_recent_sender_component_id; + la_log_unsuppress(); + la_log(LOG_INFO, "mh-dfl: Starting log, target is (%d/%d), I am (%d/%d)", sender_system_id, + sender_component_id, this_system_id, this_component_id); + if (!output_file_open()) { + return false; + } + + logging_started = true; + return true; +} + +void DataFlash_Logger::ensure_write_buffers_ownership() +{ + for (uint16_t i = 0; i < _max_write_buffers; i++) { + uint8_t count = 0; + while (_write_buffers[i].owner != NETWORK_THREAD) { + pthread_cond_signal(&_wakeup_fileio_thread); + sched_yield(); + // do we need really a sleep in here? + usleep(100000); + if (count++ > 100) { + la_log(LOG_ERR, "Stuck waiting for buffers to be freed; exitting"); + abort(); + } + } + } +} + +void DataFlash_Logger::logging_stop() +{ + logging_started = false; + // we need to own all of the _write_buffers[] to ensure the + // iothread is not currently using the output filehandle: + ensure_write_buffers_ownership(); + output_file_close(); +} + +void DataFlash_Logger::handle_message(uint64_t timestamp, mavlink_message_t &msg) +{ + most_recent_sender_system_id = msg.sysid; + most_recent_sender_component_id = msg.compid; + MAVLink_Message_Handler::handle_message(timestamp, msg); +} + +struct DataFlash_Logger::df_write_buffer * +DataFlash_Logger::next_write_buffer(const DataFlash_Logger::write_buffer_owner owner) +{ + for (uint8_t i = 0; i < _max_write_buffers; i++) { + if (_write_buffers[i].owner == owner) { + return &_write_buffers[i]; + } + } + return NULL; +} + +void DataFlash_Logger::handle_decoded_message(uint64_t T UNUSED, + mavlink_remote_log_data_block_t &msg) +{ + if (!logging_started) { + if (msg.seqno == 0) { + if (!logging_start(msg)) { + return; + } + } else { + return; + } + } + + uint64_t now = clock_gettime_us(CLOCK_MONOTONIC); + if (now - _last_data_packet_time > 100000) { + ::la_log(LOG_ERR, "mh-dfl: long time between messages (%ld)", now - _last_data_packet_time); + } + + if (_current_write_buffer == NULL) { + _current_write_buffer = next_write_buffer(NETWORK_THREAD); + if (_current_write_buffer == NULL) { + la_log(LOG_ERR, "mh-dfl: no write buffer available"); + _last_data_packet_time = now; + return; + } + } + + struct defered_write *defered_write = + &_current_write_buffer->defered_writes[_current_write_buffer->next_defered_write++]; + memcpy(defered_write->data, msg.data, MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN); + defered_write->seqno = msg.seqno; + if (_current_write_buffer->next_defered_write >= _df_write_buffer_max_defered_writes) { + // this buffer is full. Hand it to the FILEIO thread: + _current_write_buffer->owner = FILEIO_THREAD; + _current_write_buffer = NULL; + // and wake the fileio thread up: + pthread_cond_signal(&_wakeup_fileio_thread); + } + + if (clock_gettime_us(CLOCK_MONOTONIC) - now > 100000) { + ::la_log(LOG_ERR, "mh-dfl: long time to write (%ld)", + clock_gettime_us(CLOCK_MONOTONIC) - now); + } + + // queue an ack for this packet + queue_ack(msg.seqno); + + // queue nacks for gaps + queue_gap_nacks(msg.seqno); + + if (msg.seqno > highest_seqno_seen) { + if (msg.seqno - highest_seqno_seen > 100) { + la_log(LOG_ERR, "mh-dfl: large seqno gap: %ld", msg.seqno - highest_seqno_seen); + } + highest_seqno_seen = msg.seqno; + } + + _last_data_packet_time = now; +} + +void DataFlash_Logger::send_start_logging_packet() +{ + send_start_or_stop_logging_packet(true); +} + +void DataFlash_Logger::send_stop_logging_packet() +{ + send_start_or_stop_logging_packet(false); +} + +void DataFlash_Logger::send_start_or_stop_logging_packet(bool is_start) +{ + mavlink_message_t msg; + + uint8_t system_id = is_start ? target_system_id : sender_system_id; + uint8_t component_id = is_start ? target_component_id : sender_component_id; + uint32_t magic_number; + if (is_start) { + la_log(LOG_INFO, "mh-dfl: sending start packet to (%d/%d)", system_id, component_id); + magic_number = MAV_REMOTE_LOG_DATA_BLOCK_START; + } else { + la_log(LOG_INFO, "mh-dfl: sending stop packet to (%d/%d)", system_id, component_id); + magic_number = MAV_REMOTE_LOG_DATA_BLOCK_STOP; + } + mavlink_msg_remote_log_block_status_pack(this_system_id, this_component_id, &msg, system_id, + component_id, magic_number, 1); + + _mavlink_writer->send_message(msg); +} diff --git a/flightcode/dataflash_logger/dataflash_logger.h b/flightcode/dataflash_logger/dataflash_logger.h new file mode 100644 index 0000000..3e0f91d --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_logger.h @@ -0,0 +1,119 @@ +#ifndef DATAFLASH_LOGGER_H +#define DATAFLASH_LOGGER_H + +/* + * dataflash_logger + * + * Receive telemetry (mavlink) via UDP from Solo, and create dataflash log files + * + * Initiate a remote-dataflash stream + */ + +#include "INIReader.h" + +#include "mavlink_message_handler.h" +#include "mavlink_writer.h" + +#include "../mavlink/c_library/common/mavlink.h" + +class DataFlash_Logger : public MAVLink_Message_Handler +{ + +public: + void fileio_task(); + + DataFlash_Logger(MAVLink_Writer *mavlink_writer); + + enum write_buffer_owner { NETWORK_THREAD, FILEIO_THREAD }; + +private: + bool configure(INIReader *config); + + void sighup_received(); + + void idle_tenthHz(); + void idle_1Hz(); + void idle_10Hz(); + void idle_100Hz(); + bool logging_start(mavlink_remote_log_data_block_t &msg); + void logging_stop(); + void send_stop_logging_packet(); + + bool output_file_open(); + void output_file_close(); + + void ensure_write_buffers_ownership(); + + // more, smaller buffers means a lower latency to disk. A total + // of 700 defered writes gives about 10 seconds of buffering based + // on a 12k transfer rate. + static const uint8_t _max_write_buffers = 7; + static const uint16_t _df_write_buffer_max_defered_writes = 100; + struct defered_write { + uint8_t data[MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN]; + uint32_t seqno; + }; + + struct df_write_buffer { + struct defered_write defered_writes[_df_write_buffer_max_defered_writes]; + write_buffer_owner owner = NETWORK_THREAD; + uint16_t next_defered_write = 0; + } _write_buffers[_max_write_buffers]; + struct df_write_buffer *_current_write_buffer = NULL; + void write_buffer_to_disk(const struct df_write_buffer *buffer); + bool write_block_for_seqno(const uint8_t *data, uint32_t seqno); + struct df_write_buffer *next_write_buffer(const write_buffer_owner owner); + uint64_t fileio_last_log_time = 0; + + pthread_mutex_t _write_buffer_mutex; + pthread_cond_t _wakeup_fileio_thread; + pthread_t _fileio_thread; + + MAVLink_Writer *_mavlink_writer = NULL; + uint8_t this_system_id = 57; + uint8_t this_component_id = 57; + + const uint8_t target_system_id_default = 0; + const uint8_t target_component_id_default = 0; + uint8_t most_recent_sender_system_id; + uint8_t most_recent_sender_component_id; + uint8_t target_system_id; // who to send our request-for-logs to + uint8_t target_component_id; // who to send our request-for-logs to + uint8_t sender_system_id = 0; // who the logs areactually coming from + uint8_t sender_component_id = 0; // who the logs areactually coming from + + void send_start_logging_packet(); + void send_start_or_stop_logging_packet(bool is_start); + const char *_log_directory_path; + int out_fd; + bool logging_started = false; + + void handle_message(uint64_t timestamp, mavlink_message_t &msg); + void handle_decoded_message(uint64_t T, mavlink_remote_log_data_block_t &msg); + + bool make_new_log_filename(char *buffer, uint8_t bufferlen); + + void send_response(uint32_t seqno, bool status); + void push_response_queue(); + +#define RESPONSE_QUEUE_LENGTH 128 + struct packet_status { + uint32_t seqno; + bool status; // ack == true + } responses[RESPONSE_QUEUE_LENGTH]; + uint8_t response_queue_head = 0; + uint8_t response_queue_tail = 0; + + uint32_t highest_seqno_seen = 0; + uint64_t _last_data_packet_time = 0; + + /* if we lose > this many packets we do not nack anything in that gap: */ + uint8_t seqno_gap_nack_threshold; + + void queue_response(uint32_t seqno, bool status); + void queue_nack(uint32_t seqno); + void queue_ack(uint32_t seqno); + void queue_gap_nacks(uint32_t seqno); +}; + +#endif diff --git a/flightcode/dataflash_logger/dataflash_logger_program.cpp b/flightcode/dataflash_logger/dataflash_logger_program.cpp new file mode 100644 index 0000000..0e5325a --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_logger_program.cpp @@ -0,0 +1,167 @@ +#include "dataflash_logger_program.h" + +#include "dataflash_logger.h" +#include "heart.h" +#include "mavlink_reader.h" +#include "telem_forwarder_client.h" +#include "telem_serial.h" + +#include "la-log.h" + +#include +#include + +const char *DataFlash_Logger_Program::program_name() +{ + if (_argv == NULL) { + return "[Unknown]"; + } + return _argv[0]; +} + +void DataFlash_Logger_Program::usage() +{ + ::printf("Usage:\n"); + ::printf("%s [OPTION] [FILE]\n", program_name()); + ::printf(" -c filepath use config file filepath\n"); + ::printf(" -h display usage information\n"); + ::printf(" -d debug mode\n"); + ::printf("\n"); + ::printf("Example: %s\n", program_name()); + exit(0); +} + +DataFlash_Logger_Program logger; + +void sighup_handler(int signal) +{ + logger.sighup_handler(signal); +} + +void DataFlash_Logger_Program::do_idle_callbacks() +{ + reader->do_idle_callbacks(); +} + +void DataFlash_Logger_Program::sighup_received_tophalf() +{ + reader->sighup_handler(); +} + +uint32_t DataFlash_Logger_Program::select_timeout_us() +{ + if (_writer->any_data_to_send()) { + return 0; + } + return Common_Tool::select_timeout_us(); +} + +void DataFlash_Logger_Program::pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds) +{ + client->pack_select_fds(fds_read, fds_write, fds_err, nfds); +} + +void DataFlash_Logger_Program::do_writer_sends() +{ + client->do_writer_sends(); +} + +void DataFlash_Logger_Program::handle_select_fds(fd_set &fds_read, fd_set &fds_write, + fd_set &fds_err, uint8_t &nfds) +{ + client->handle_select_fds(fds_read, fds_write, fds_err, nfds); + + // FIXME: find a more interesting way of doing this... we should + // probably rejig things so that the client is a mavlink_reader + // and simply produces mavlink_message_t's itself, rather than us + // handing off the a dedicated parser object here. + reader->feed(client->_recv_buf, client->_recv_buflen_content); + client->_recv_buflen_content = 0; + + // handle data *to* e.g. telem_forwarder + do_writer_sends(); +} + +void DataFlash_Logger_Program::run() +{ + init_config(); + + if (!debug_mode) { + la_log_syslog_open(); + } + + la_log(LOG_INFO, "dataflash_logger starting: built " __DATE__ " " __TIME__); + signal(SIGHUP, ::sighup_handler); + + reader = new MAVLink_Reader(config()); + if (reader == NULL) { + la_log(LOG_ERR, "Failed to create reader from (%s)\n", config_filename); + exit(1); + } + + if (serial_port) { + client = new Telem_Serial(_client_recv_buf, sizeof(_client_recv_buf)); + } else { + client = new Telem_Forwarder_Client(_client_recv_buf, sizeof(_client_recv_buf)); + } + client->configure(config()); + client->init(); + + _writer = new MAVLink_Writer(config()); + _writer->add_client(client); + if (_writer == NULL) { + la_log(LOG_ERR, "Failed to create writer from (%s)\n", config_filename); + exit(1); + } + + // instantiate message handlers: + DataFlash_Logger *dataflash_logger = new DataFlash_Logger(_writer); + if (dataflash_logger != NULL) { + reader->add_message_handler(dataflash_logger, "DataFlash_Logger"); + } else { + la_log(LOG_INFO, "Failed to create dataflash logger"); + } + + Heart *heart = new Heart(_writer); + if (heart != NULL) { + reader->add_message_handler(heart, "Heart"); + } else { + la_log(LOG_INFO, "Failed to create heart"); + } + + return select_loop(); +} + +void DataFlash_Logger_Program::parse_arguments(int argc, char *argv[]) +{ + int opt; + _argc = argc; + _argv = argv; + + while ((opt = getopt(argc, argv, "hc:ds")) != -1) { + switch (opt) { + case 'h': + usage(); + break; + case 'c': + config_filename = optarg; + break; + case 'd': + debug_mode = true; + break; + case 's': + serial_port = true; + break; + } + } +} + +/* +* main - entry point +*/ +int main(int argc, char *argv[]) +{ + logger.parse_arguments(argc, argv); + logger.run(); +} diff --git a/flightcode/dataflash_logger/dataflash_logger_program.h b/flightcode/dataflash_logger/dataflash_logger_program.h new file mode 100644 index 0000000..9c8ee78 --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_logger_program.h @@ -0,0 +1,49 @@ +#include "common_tool.h" + +#include "mavlink_reader.h" +#include "mavlink_writer.h" +#include "telem_client.h" + +class DataFlash_Logger_Program : public Common_Tool +{ +public: + DataFlash_Logger_Program() : Common_Tool() + { + } + + void run(); + + void parse_arguments(int argc, char *argv[]); + const char *program_name(); + + void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds) override; + void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds) override; + +private: + void usage(); + void sighup_received_tophalf() override; + void do_idle_callbacks() override; + uint32_t select_timeout_us() override; + + void do_writer_sends(); + + MAVLink_Reader *reader; + MAVLink_Writer *_writer; + + long _argc = 0; + char **_argv = NULL; + + uint8_t _client_recv_buf[512] = {}; // FIXME constant was TELEM_PKT_MAX + + static const uint32_t _client_buflen = 65536; // FIXME constant + uint32_t _client_buflen_start = 0; + uint32_t _client_buflen_stop = 0; + + Telem_Client *client = NULL; + bool debug_mode = false; + bool serial_port = false; + // uint8_t _writer_buf[_writer_buflen] = { }; + uint32_t canary = 9876543; +}; diff --git a/flightcode/dataflash_logger/dataflash_message_handler.cpp b/flightcode/dataflash_logger/dataflash_message_handler.cpp new file mode 100644 index 0000000..a90844a --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_message_handler.cpp @@ -0,0 +1 @@ +#include "dataflash_message_handler.h" diff --git a/flightcode/dataflash_logger/dataflash_message_handler.h b/flightcode/dataflash_logger/dataflash_message_handler.h new file mode 100644 index 0000000..b71e396 --- /dev/null +++ b/flightcode/dataflash_logger/dataflash_message_handler.h @@ -0,0 +1,31 @@ +#ifndef DATAFLASH_MESSAGE_HANDLER_H +#define DATAFLASH_MESSAGE_HANDLER_H + +#include + +/* + * dataflash_message_handler + * + * A base class for objects which process dataflash messages and + * possibly send responses + * + */ + +#include + +#include "INIReader.h" + +#include "message_handler.h" + +class DataFlash_Message_Handler : public Message_Handler +{ +public: + virtual void handle_format_message_received(const char *name, const struct log_Format &format, + const char *msg) = 0; + virtual void handle_message_received(const struct log_Format &format, const uint8_t *msg) = 0; + +protected: +private: +}; + +#endif diff --git a/flightcode/dataflash_logger/format_reader.cpp b/flightcode/dataflash_logger/format_reader.cpp new file mode 100644 index 0000000..a1b7460 --- /dev/null +++ b/flightcode/dataflash_logger/format_reader.cpp @@ -0,0 +1,61 @@ +#include "format_reader.h" + +#include "la-log.h" + +void Format_Reader::do_idle_callbacks() +{ + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (next_100hz_time <= now_us) { + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->idle_100Hz(); + } + next_100hz_time += 10000; + } + if (next_10hz_time <= now_us) { + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->idle_10Hz(); + } + next_10hz_time += 100000; + } + if (next_1hz_time <= now_us) { + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->idle_1Hz(); + } + next_1hz_time += 1000000; + } + if (next_tenthhz_time <= now_us) { + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->idle_tenthHz(); + } + next_tenthhz_time += 10000000; + } +} + +bool Format_Reader::add_message_handler(Message_Handler *handler, const char *handler_name) +{ + if (MAX_MESSAGE_HANDLERS - next_message_handler < 2) { + la_log(LOG_INFO, "Insufficient message handler slots (MAX=%d) (next=%d)?!", + MAX_MESSAGE_HANDLERS, next_message_handler); + return false; + } + + if (!handler->configure(_config)) { + la_log(LOG_INFO, "Failed to configure (%s)", handler_name); + return false; + } + + message_handler[next_message_handler++] = handler; + return true; +} + +void Format_Reader::clear_message_handlers() +{ + next_message_handler = 0; +} + +void Format_Reader::sighup_handler() +{ + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->sighup_received(); + } +} diff --git a/flightcode/dataflash_logger/format_reader.h b/flightcode/dataflash_logger/format_reader.h new file mode 100644 index 0000000..2705b8a --- /dev/null +++ b/flightcode/dataflash_logger/format_reader.h @@ -0,0 +1,49 @@ +#ifndef FORMAT_READER_H +#define FORMAT_READER_H + +#include "INIReader.h" +#include "util.h" + +#include "message_handler.h" + +class Format_Reader +{ +public: + Format_Reader(INIReader *config) : _config(config) + { + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + next_tenthhz_time = now_us; + next_1hz_time = now_us; + next_10hz_time = now_us; + next_100hz_time = now_us; + }; + + void do_idle_callbacks(); + + virtual bool add_message_handler(Message_Handler *handler, const char *handler_name); + virtual void clear_message_handlers(); + + virtual void sighup_handler(); + + virtual uint32_t feed(const uint8_t *buf, const uint32_t len) = 0; + + virtual void end_of_log(){}; + +protected: + INIReader *_config; + +// FIXME: Scope +#define MAX_MESSAGE_HANDLERS 10 + uint8_t next_message_handler = 0; + Message_Handler *message_handler[MAX_MESSAGE_HANDLERS]; + +private: + bool sighup_received = false; + + uint64_t next_tenthhz_time; + uint64_t next_1hz_time; + uint64_t next_10hz_time; + uint64_t next_100hz_time; +}; + +#endif diff --git a/flightcode/dataflash_logger/heart.cpp b/flightcode/dataflash_logger/heart.cpp new file mode 100644 index 0000000..1e7bd69 --- /dev/null +++ b/flightcode/dataflash_logger/heart.cpp @@ -0,0 +1,39 @@ +#include "heart.h" + +#include "util.h" +#include "la-log.h" + +void Heart::idle_10Hz() +{ + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (last_heartbeat_time + heartbeat_interval < now_us) { + last_heartbeat_time = now_us; + beat(); + } +} + +bool Heart::configure(INIReader *config) +{ + if (!MAVLink_Message_Handler::configure(config)) { + return false; + } + return true; +} + +void Heart::beat() +{ + mavlink_message_t msg; + + uint8_t type = MAV_TYPE_GCS; + uint8_t autopilot = MAV_AUTOPILOT_INVALID; + uint8_t base_mode = 0; + uint32_t custom_mode = 0; + uint8_t system_status = 0; + + la_log(LOG_DEBUG, "mh-h: sending heartbeat"); + + mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, + custom_mode, system_status); + + _mavlink_writer->send_message(msg); +} diff --git a/flightcode/dataflash_logger/heart.h b/flightcode/dataflash_logger/heart.h new file mode 100644 index 0000000..243069d --- /dev/null +++ b/flightcode/dataflash_logger/heart.h @@ -0,0 +1,34 @@ +#ifndef HEART_H +#define HEART_H + +/* + * heart + * + * Periodically send mavlink heartbeats to our upstream connection + * + */ + +#include "mavlink_message_handler.h" +#include "mavlink_writer.h" + +class Heart : public MAVLink_Message_Handler +{ + +public: + Heart(MAVLink_Writer *mavlink_writer) + : MAVLink_Message_Handler(), _mavlink_writer(mavlink_writer), last_heartbeat_time(0), + heartbeat_interval(5000000) // microseconds + { + } + + void idle_10Hz(); + bool configure(INIReader *config); + +private: + MAVLink_Writer *_mavlink_writer = NULL; + uint64_t last_heartbeat_time; + const uint32_t heartbeat_interval; + void beat(); +}; + +#endif diff --git a/flightcode/dataflash_logger/la-log.cpp b/flightcode/dataflash_logger/la-log.cpp new file mode 100644 index 0000000..948a748 --- /dev/null +++ b/flightcode/dataflash_logger/la-log.cpp @@ -0,0 +1,82 @@ +#include "la-log.h" + +// Caution: the functions exposed in here are called by different +// threads in the same process. + +LALog lalog; + +// globa functions for convenience +void la_log_syslog_open() +{ + lalog.syslog_open(); +} + +void la_log_unsuppress() +{ + lalog.unsupress(); +} + +void la_log(int priority, const char *format, ...) +{ + va_list ap; + va_start(ap, format); + lalog.log_ap(priority, format, ap); + va_end(ap); +} + +void LALog::syslog_open() +{ + openlog("dl", LOG_NDELAY, LOG_LOCAL1); + + use_syslog = true; +} + +void LALog::log(const int priority, const char *format, ...) +{ + va_list ap; + va_start(ap, format); + log_ap(priority, format, ap); + va_end(ap); +} + +bool LALog::should_suppress() +{ + return _message_count_this_time_period >= _max_messages_per_time_period; +} + +void LALog::unsupress() +{ + _suppressing = false; + _message_count_this_time_period = 0; + _time_period_start = time(NULL); + if (_suppressed_message_count) { + // hmmm. recursion. hmmm. + log(LOG_ERR, "%d messages supressed", _suppressed_message_count); + _message_count_this_time_period--; // *cough* let one + // *message through + // *apart from us.... + _suppressed_message_count = 0; + } +} +void LALog::log_ap(int priority, const char *format, va_list ap) +{ + if (time(NULL) - _time_period_start > _time_period) { + unsupress(); + } + if (should_suppress()) { + _suppressing = true; + } + if (suppressing()) { + _suppressed_message_count++; + return; + } + + _message_count_this_time_period++; + + if (use_syslog) { + vsyslog(priority, format, ap); + } else { + vfprintf(stderr, format, ap); + fprintf(stderr, "%s", "\n"); + } +} diff --git a/flightcode/dataflash_logger/la-log.h b/flightcode/dataflash_logger/la-log.h new file mode 100644 index 0000000..1a38906 --- /dev/null +++ b/flightcode/dataflash_logger/la-log.h @@ -0,0 +1,45 @@ +#ifndef _LA_LOG_H +#define _LA_LOG_H + +#include +#include +#include +#include +#include + +void la_log_syslog_open(); +void la_log(int priority, const char *format, ...); +void la_log_unsuppress(); + +class LALog +{ +public: + LALog() : _time_period_start(time(NULL)) + { + } + void syslog_open(); + + void log(int priority, const char *format, ...); + void log_ap(int priority, const char *format, va_list ap); + + void unsupress(); + bool should_suppress(); + bool suppressing() + { + return _suppressing; + } + +private: + // really rough rate limiting for log messages. We could keep a + // hash here of formats and selectively supress based on format. + bool _suppressing = false; + const uint8_t _time_period = 5; // seconds + const uint8_t _max_messages_per_time_period = 10; + uint32_t _suppressed_message_count = 0; + uint8_t _message_count_this_time_period = 0; + time_t _time_period_start; + + bool use_syslog = false; +}; + +#endif diff --git a/flightcode/dataflash_logger/mavlink_message_handler.cpp b/flightcode/dataflash_logger/mavlink_message_handler.cpp new file mode 100644 index 0000000..83d7725 --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_message_handler.cpp @@ -0,0 +1,119 @@ +#include "mavlink_message_handler.h" + +#include "la-log.h" +#include + +// void MAVLink_Message_Handler::send_message_to_telem_forwarder(mavlink_message_t &msg) +// { +// uint8_t sendbuf[512]; + +// uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t*)sendbuf,&msg); + +// if (sendto(_fd_telem_forwarder, sendbuf, messageLen, 0, (struct sockaddr +// *)_sa_telemetry_forwarder, sizeof(struct sockaddr)) == -1) { +// la_log(LOG_INFO, "Failed sendto: %s", strerror(errno)); +// } +// } + +void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg) +{ + // ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid); + switch (msg.msgid) { + case MAVLINK_MSG_ID_AHRS2: { + mavlink_ahrs2_t decoded; + mavlink_msg_ahrs2_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_ATTITUDE: { + mavlink_attitude_t decoded; + mavlink_msg_attitude_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { + mavlink_ekf_status_report_t decoded; + mavlink_msg_ekf_status_report_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { + mavlink_global_position_int_t decoded; + mavlink_msg_global_position_int_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_gps_raw_int_t decoded; + mavlink_msg_gps_raw_int_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t decoded; + mavlink_msg_heartbeat_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_MOUNT_STATUS: { + mavlink_mount_status_t decoded; + mavlink_msg_mount_status_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { + mavlink_nav_controller_output_t decoded; + mavlink_msg_nav_controller_output_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_PARAM_VALUE: { + mavlink_param_value_t decoded; + mavlink_msg_param_value_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: { + mavlink_remote_log_data_block_t decoded; + mavlink_msg_remote_log_data_block_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_SCALED_PRESSURE: { + mavlink_scaled_pressure_t decoded; + mavlink_msg_scaled_pressure_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_SCALED_PRESSURE2: { + mavlink_scaled_pressure2_t decoded; + mavlink_msg_scaled_pressure2_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { + mavlink_servo_output_raw_t decoded; + mavlink_msg_servo_output_raw_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_STATUSTEXT: { + mavlink_statustext_t decoded; + mavlink_msg_statustext_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_SYS_STATUS: { + mavlink_sys_status_t decoded; + mavlink_msg_sys_status_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + case MAVLINK_MSG_ID_VFR_HUD: { + mavlink_vfr_hud_t decoded; + mavlink_msg_vfr_hud_decode(&msg, &decoded); + handle_decoded_message(timestamp, decoded); + break; + } + } +} diff --git a/flightcode/dataflash_logger/mavlink_message_handler.h b/flightcode/dataflash_logger/mavlink_message_handler.h new file mode 100644 index 0000000..d65acaa --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_message_handler.h @@ -0,0 +1,98 @@ +#ifndef MAVLINK_MESSAGE_HANDLER_H +#define MAVLINK_MESSAGE_HANDLER_H + +/* + * mavlink_message_handler + * + * A base class for objects which process mavlink messages and + * possibly send responses + * + */ + +#include + +#include "INIReader.h" +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "../mavlink/c_library/common/mavlink.h" + +#include + +#include "message_handler.h" + +#define UNUSED __attribute__((unused)) + +class MAVLink_Message_Handler : public Message_Handler +{ +public: + MAVLink_Message_Handler() : Message_Handler() + { + } + + virtual bool configure(INIReader *config) + { + system_id = config->GetInteger("dflogger", "system_id", 254); + component_id = config->GetInteger("dflogger", "component_id", 17); + return true; + } + + virtual void handle_message(uint64_t timestamp, mavlink_message_t &msg); + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_ahrs2_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_attitude_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_ekf_status_report_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_gps_raw_int_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, + mavlink_global_position_int_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_heartbeat_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_mount_status_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, + mavlink_nav_controller_output_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_param_value_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, + mavlink_remote_log_data_block_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_scaled_pressure_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_scaled_pressure2_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_servo_output_raw_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_sys_status_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_statustext_t &msg UNUSED) + { + } + virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_vfr_hud_t &msg UNUSED) + { + } + +protected: + uint8_t system_id; + uint8_t component_id; + +private: +}; + +#endif diff --git a/flightcode/dataflash_logger/mavlink_reader.cpp b/flightcode/dataflash_logger/mavlink_reader.cpp new file mode 100644 index 0000000..70709be --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_reader.cpp @@ -0,0 +1,82 @@ +#include "mavlink_reader.h" + +#include +#include // for perror +#include // for abort + +#include // for usleep + +#include +#include + +#include +#include +#include + +#include "analyzer_util.h" +#include "la-log.h" + +/* This is used to prevent swamping the log with error messages if + something unexpected happens. + Returns -1 if we cannot log an error now, or returns the number of + messages skipped due to rate limiting if we can, i.e. a return of + 2 means log, and we have skipped 2 messages due to rate limiting. */ +int MAVLink_Reader::can_log_error() +{ + unsigned ret_val; + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + if ((now_us - err_time_us) < err_interval_us) { + /* can't log */ + err_skipped++; + return -1; + } + /* yes; say we can and set err_time_us assuming we do log something */ + err_time_us = now_us; + ret_val = err_skipped; + err_skipped = 0; + return ret_val; +} + +void MAVLink_Reader::handle_message_received(uint64_t timestamp, mavlink_message_t msg) +{ + for (int i = 0; i < next_message_handler; i++) { + ((MAVLink_Message_Handler *)message_handler[i])->handle_message(timestamp, msg); + } +} + +uint32_t MAVLink_Reader::feed(const uint8_t *buf, const uint32_t len) +{ + for (uint32_t i = 0; i < len; i++) { + // ::printf("Read (%d)\n", i); + if (is_tlog() && !done_timestamp) { + timestamp <<= 8; + timestamp |= (uint8_t)(buf[i]); + if (timestamp_offset++ == 7) { + done_timestamp = true; + // ::printf("timestamp (%lu)\n", timestamp); + timestamp_offset = 0; + } + } else { + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &mav_msg, &mav_status)) { + if (!is_tlog()) { + timestamp = now(); + } + // printf("message received at %d (parse_errors=%u)\n", i, mav_status.parse_error); + handle_message_received(timestamp, mav_msg); + packet_count++; + done_timestamp = false; + timestamp = 0; + } else { + // printf(" %u state: %u\n", i, mav_status.parse_state); + } + } + } + return len; // at the moment we parse everything we receive +} + +void MAVLink_Reader::end_of_log() +{ + for (int i = 0; i < next_message_handler; i++) { + message_handler[i]->end_of_log(packet_count); + } +} diff --git a/flightcode/dataflash_logger/mavlink_reader.h b/flightcode/dataflash_logger/mavlink_reader.h new file mode 100644 index 0000000..0dbdd45 --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_reader.h @@ -0,0 +1,61 @@ +#ifndef MAVLINK_READER_H +#define MAVLINK_READER_H + +#include +#include + +#include "INIReader.h" +#include "util.h" + +#include "mavlink_message_handler.h" + +#include "format_reader.h" + +/* A mavlink packet should be limited to 6+255+2 = 263 bytes + 6 byte header, 255 max bytes in payload, 2 byte crc */ +#define TELEM_PKT_MAX 512 + +class MAVLink_Reader : public Format_Reader +{ +public: + MAVLink_Reader(INIReader *config) + : Format_Reader(config), err_skipped(0), err_time_us(0), /* last time we logged */ + err_interval_us(1000000), /* once per second max */ + _is_tlog(false) + { + } + + uint32_t feed(const uint8_t *buf, const uint32_t len) override; + + void set_is_tlog(bool value) + { + _is_tlog = value; + } + bool is_tlog() + { + return _is_tlog; + } + +protected: + void end_of_log() override; + +private: + int can_log_error(); + + void handle_message_received(uint64_t timestamp, mavlink_message_t msg); + + uint16_t err_skipped; + uint64_t err_time_us; + uint64_t err_interval_us; + + uint32_t packet_count = 0; + uint8_t timestamp_offset = 0; + bool done_timestamp = false; + uint64_t timestamp = 0; + mavlink_message_t mav_msg; + mavlink_status_t mav_status; + + bool _is_tlog; +}; + +#endif diff --git a/flightcode/dataflash_logger/mavlink_writer.cpp b/flightcode/dataflash_logger/mavlink_writer.cpp new file mode 100644 index 0000000..51f5b33 --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_writer.cpp @@ -0,0 +1,29 @@ +#include "mavlink_writer.h" + +#include + +void MAVLink_Writer::add_client(Telem_Client *client) +{ + clients.push_back(client); +} + +void MAVLink_Writer::send_message(const mavlink_message_t &msg) +{ + // std::for_each(clients.begin(), clients.end(), send_message(msg)); + std::for_each(clients.begin(), clients.end(), [msg](Telem_Client *c) { c->send_message(msg); }); + // for (std::vector::iterator it = clients.begin(); + // it != clients.end(); + // it++) { + // (*it)->send_message(msg); + // } +} + +bool MAVLink_Writer::any_data_to_send() +{ + for (std::vector< Telem_Client * >::iterator it = clients.begin(); it != clients.end(); it++) { + if ((*it)->any_data_to_send()) { + return true; + } + } + return false; +} diff --git a/flightcode/dataflash_logger/mavlink_writer.h b/flightcode/dataflash_logger/mavlink_writer.h new file mode 100644 index 0000000..9c3b1b0 --- /dev/null +++ b/flightcode/dataflash_logger/mavlink_writer.h @@ -0,0 +1,27 @@ +#ifndef _MAVLINK_WRITER_H +#define _MAVLINK_WRITER_H + +#include + +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "INIReader.h" +#include "telem_client.h" + +#define UNUSED __attribute__((unused)) + +class MAVLink_Writer +{ +public: + MAVLink_Writer(INIReader *config UNUSED) + { + } + void send_message(const mavlink_message_t &msg); + + void add_client(Telem_Client *); + bool any_data_to_send(); + +private: + std::vector< Telem_Client * > clients; +}; + +#endif diff --git a/flightcode/dataflash_logger/message_handler.cpp b/flightcode/dataflash_logger/message_handler.cpp new file mode 100644 index 0000000..e69de29 diff --git a/flightcode/dataflash_logger/message_handler.h b/flightcode/dataflash_logger/message_handler.h new file mode 100644 index 0000000..4220693 --- /dev/null +++ b/flightcode/dataflash_logger/message_handler.h @@ -0,0 +1,39 @@ +#ifndef MESSAGE_HANDLER_H +#define MESSAGE_HANDLER_H + +#define UNUSED __attribute__((unused)) + +class Message_Handler +{ +public: + friend class Format_Reader; + + virtual bool configure(INIReader *config UNUSED) + { + return true; + } + + virtual void sighup_received() + { + } + + virtual void end_of_log(uint32_t packet_count UNUSED) + { + } + +protected: + virtual void idle_tenthHz() + { + } + virtual void idle_1Hz() + { + } + virtual void idle_10Hz() + { + } + virtual void idle_100Hz() + { + } +}; + +#endif diff --git a/flightcode/dataflash_logger/telem_client.cpp b/flightcode/dataflash_logger/telem_client.cpp new file mode 100644 index 0000000..5ff97ec --- /dev/null +++ b/flightcode/dataflash_logger/telem_client.cpp @@ -0,0 +1,13 @@ +#include "telem_client.h" + +uint32_t Telem_Client::send_buffer_used() +{ + if (_send_buf_stop >= _send_buf_start) { + return _send_buf_stop - _send_buf_start; + } + return send_buf_size() - _send_buf_start + _send_buf_stop; +} +uint32_t Telem_Client::send_buffer_space_free() +{ + return send_buf_size() - send_buffer_used(); +} diff --git a/flightcode/dataflash_logger/telem_client.h b/flightcode/dataflash_logger/telem_client.h new file mode 100644 index 0000000..5d0124e --- /dev/null +++ b/flightcode/dataflash_logger/telem_client.h @@ -0,0 +1,44 @@ +#ifndef _TELEM_CLIENT_H +#define _TELEM_CLIENT_H + +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include +#include "INIReader.h" + +class Telem_Client +{ +public: + Telem_Client(uint8_t *recv_buf, uint32_t recv_buflen) + : _recv_buf(recv_buf), _recv_buflen(recv_buflen) + { + } + + virtual void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds) = 0; + virtual void configure(INIReader *config) = 0; + virtual void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, + uint8_t &nfds) = 0; + + virtual void init() = 0; + + virtual void do_writer_sends() = 0; + virtual bool send_message(const mavlink_message_t &message) = 0; + virtual bool any_data_to_send() = 0; + + virtual uint32_t send_buffer_space_free(); + virtual uint32_t send_buffer_used(); + + uint32_t _send_buf_start = 0; + uint32_t _send_buf_stop = 0; + + // FIXME: scope + uint8_t *_recv_buf; // receive buffer + uint32_t _recv_buflen; // receive buffer len + uint32_t _recv_buflen_content = 0; + + virtual uint32_t send_buf_size() const = 0; + +private: +}; + +#endif diff --git a/flightcode/dataflash_logger/telem_forwarder_client.cpp b/flightcode/dataflash_logger/telem_forwarder_client.cpp new file mode 100644 index 0000000..5606c26 --- /dev/null +++ b/flightcode/dataflash_logger/telem_forwarder_client.cpp @@ -0,0 +1,174 @@ +#include "telem_forwarder_client.h" + +#include +// #include +// #include +// #include + +#include "la-log.h" + +#define UNUSED __attribute__((unused)) + +void Telem_Forwarder_Client::pack_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, + fd_set &fds_err, uint8_t &nfds) +{ + FD_SET(fd_telem_forwarder, &fds_read); + FD_SET(fd_telem_forwarder, &fds_err); + + if (fd_telem_forwarder >= nfds) { + nfds = fd_telem_forwarder + 1; + } +} + +void Telem_Forwarder_Client::handle_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, + fd_set &fds_err, uint8_t &nfds UNUSED) +{ + /* check for packets from telem_forwarder */ + if (FD_ISSET(fd_telem_forwarder, &fds_err)) { + FD_CLR(fd_telem_forwarder, &fds_err); + la_log(LOG_ERR, "select(fd_telem_forwarder): %s", strerror(errno)); + } + + if (FD_ISSET(fd_telem_forwarder, &fds_read)) { + FD_CLR(fd_telem_forwarder, &fds_read); + _recv_buflen_content = handle_recv(); + // ::fprintf(stderr, "received %u bytes\n", _buflen_content); + } +} + +/* +* create_and_bind - create a socket and bind it to a local UDP port +* +* Used to create the socket on the upstream side that receives from and sends +* to telem_forwarder +* +* Returns fd on success, -1 on error. +*/ +void Telem_Forwarder_Client::create_and_bind() +{ + int fd; + struct sockaddr_in sa; + + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + perror("socket"); + abort(); + } + + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_addr.s_addr = htonl(INADDR_ANY); + sa.sin_port = 0; // we don't care what our port is + + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + perror("bind"); + abort(); + } + + fd_telem_forwarder = fd; +} /* create_and_bind */ + +void Telem_Forwarder_Client::pack_telem_forwarder_sockaddr(INIReader *config) +{ + // uint16_t tf_port = config->GetInteger("solo", "telem_forward_port", 14560); + // std::string ip = config->Get("solo", "soloIp", "10.1.1.10"); + uint16_t tf_port = config->GetInteger("solo", "telem_forward_port", 14560); + std::string ip = config->Get("solo", "soloIp", "127.0.0.1"); + + la_log(LOG_INFO, "df-tfc: connecting to telem-forwarder at %s:%u", ip.c_str(), tf_port); + memset(&sa_tf, 0, sizeof(sa_tf)); + sa_tf.sin_family = AF_INET; + // sa_tf.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + inet_aton(ip.c_str(), &sa_tf.sin_addr); // useful for debugging + sa_tf.sin_port = htons(tf_port); +} + +bool Telem_Forwarder_Client::sane_telem_forwarder_packet(uint8_t *pkt, uint16_t pktlen) +{ + if (sa.sin_addr.s_addr != sa_tf.sin_addr.s_addr) { + la_log(LOG_ERR, "received packet not from solo (0x%08x)", sa.sin_addr.s_addr); + return false; + } + if (pktlen < 8) { + la_log(LOG_ERR, "received runt packet (%d bytes)", pktlen); + return false; + } + if (pkt[0] != 254) { + la_log(LOG_ERR, "received bad magic (0x%02x)", pkt[0]); + return false; + } + if (pkt[1] != (pktlen - 8)) { + la_log(LOG_ERR, "inconsistent length (%u, %u)", pkt[1], pktlen); + return false; + } + return true; +} + +uint32_t Telem_Forwarder_Client::handle_recv() +{ + // ::printf("Receiving packet into position %u\n", _buflen_content); + /* packet from telem_forwarder */ + socklen_t sa_len = sizeof(sa); + uint16_t res = + recvfrom(fd_telem_forwarder, &_recv_buf[_recv_buflen_content], + _recv_buflen - _recv_buflen_content, 0, (struct sockaddr *)&sa, &sa_len); + + /* We get one mavlink packet per udp datagram. Sanity checks here + are: must be from solo's IP and have a valid mavlink header. */ + // FIXME: we don't necessarily get just one packet/buffer! + // ::fprintf(stderr, "handle_recv\n"); + if (!sane_telem_forwarder_packet(_recv_buf, res)) { + return 0; + } + + return res; +} + +bool Telem_Forwarder_Client::send_message(const mavlink_message_t &msg) +{ + if (send_buffer_space_free() < 1) { + // dropped_packets++; + return false; + } + memcpy(&_send_buf[_send_buf_stop++], (char *)&msg, sizeof(msg)); + if (_send_buf_stop >= send_buf_size()) { + _send_buf_stop = 0; + } + return true; +} + +void Telem_Forwarder_Client::do_writer_sends() +{ + char buf[1024]; // large enough... + + while (_send_buf_start != _send_buf_stop) { + mavlink_message_t &msg = _send_buf[_send_buf_start]; + uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg); + + int32_t bytes_sent = sendto(fd_telem_forwarder, buf, messageLen, 0, + (struct sockaddr *)&sa_tf, sizeof(struct sockaddr)); + if (bytes_sent == -1) { + la_log(LOG_INFO, "Failed sendto: %s", strerror(errno)); + // we drop the message anyway! + } + _send_buf_start++; + if (_send_buf_start >= send_buf_size()) { + _send_buf_start = 0; + } + } + + return; +} + +void Telem_Forwarder_Client::configure(INIReader *config) +{ + /* prepare sockaddr used to contact telem_forwarder */ + pack_telem_forwarder_sockaddr(config); +} + +void Telem_Forwarder_Client::init() +{ + /* Prepare a port to receive and send data to/from telem_forwarder */ + /* does not return on failure */ + create_and_bind(); +} diff --git a/flightcode/dataflash_logger/telem_forwarder_client.h b/flightcode/dataflash_logger/telem_forwarder_client.h new file mode 100644 index 0000000..5103c6f --- /dev/null +++ b/flightcode/dataflash_logger/telem_forwarder_client.h @@ -0,0 +1,47 @@ +#include "INIReader.h" + +#include +#include +#include + +#include "telem_client.h" + +class Telem_Forwarder_Client : public Telem_Client +{ +public: + Telem_Forwarder_Client(uint8_t *recv_buf, uint32_t recv_buflen) + : Telem_Client(recv_buf, recv_buflen) + { + } + + uint32_t handle_recv(); + void init() override; + + void configure(INIReader *config); + void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds); + void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds); + + void do_writer_sends(); + bool send_message(const mavlink_message_t &msg); + bool any_data_to_send() + { + return _send_buf_start != _send_buf_stop; + } + +private: + int fd_telem_forwarder = -1; + + struct sockaddr_in sa; // our send-from address + struct sockaddr_in sa_tf; /* telem_forwarder's address */ + + void create_and_bind(); + void pack_telem_forwarder_sockaddr(INIReader *config); + bool sane_telem_forwarder_packet(uint8_t *pkt, uint16_t bpktlen); + + /* send buffer stuff: */ + mavlink_message_t _send_buf[256]; // way too bug? + uint32_t send_buf_size() const + { + return 256; + } +}; diff --git a/flightcode/dataflash_logger/telem_serial.cpp b/flightcode/dataflash_logger/telem_serial.cpp new file mode 100644 index 0000000..b29fcc0 --- /dev/null +++ b/flightcode/dataflash_logger/telem_serial.cpp @@ -0,0 +1,217 @@ +#include "telem_serial.h" + +#include "la-log.h" + +#include +#include +#include +#include + +#define UNUSED __attribute__((unused)) + +void Telem_Serial::init() +{ + open_serial_port(); +} + +void Telem_Serial::pack_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, fd_set &fds_err, + uint8_t &nfds) +{ + FD_SET(fd, &fds_read); + FD_SET(fd, &fds_err); + + if (fd >= nfds) { + nfds = fd + 1; + } +} + +void Telem_Serial::handle_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, fd_set &fds_err, + uint8_t &nfds UNUSED) +{ + /* check for packets from telem_forwarder */ + if (FD_ISSET(fd, &fds_err)) { + FD_CLR(fd, &fds_err); + la_log(LOG_ERR, "select(%d): %s", fd, strerror(errno)); + } + + if (FD_ISSET(fd, &fds_read)) { + FD_CLR(fd, &fds_read); + _recv_buflen_content = handle_read(); + // ::fprintf(stderr, "received %u bytes\n", _buflen_content); + } +} + +// from serial_setup in telem_forward +void Telem_Serial::open_serial_port() +{ + fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (fd < 0) { + la_log(LOG_ERR, "unable to open serial port %s", serialPortName.c_str()); + abort(); + } + + // Configure port for 8N1 transmission + struct termios options; + + tcgetattr(fd, &options); // Gets the current options for the port + // Set the output baud rate + switch (serialBaud) { + case 1200: + cfsetspeed(&options, B1200); + break; + case 2400: + cfsetspeed(&options, B2400); + break; + case 4800: + cfsetspeed(&options, B4800); + break; + case 9600: + cfsetspeed(&options, B9600); + break; + case 19200: + cfsetspeed(&options, B19200); + break; + case 38400: + cfsetspeed(&options, B38400); + break; + case 57600: + cfsetspeed(&options, B57600); + break; + case 115200: + cfsetspeed(&options, B115200); + break; + case 500000: + cfsetspeed(&options, B500000); + break; + case 921600: + cfsetspeed(&options, B921600); + break; + case 1500000: + cfsetspeed(&options, B1500000); + break; + default: + syslog(LOG_ERR, "unsupported baud rate %d", serialBaud); + return; + } + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= (CS8 | CLOCAL); + + if (serialFlow) { + options.c_cflag |= CRTSCTS; // hardware flow control + } else { + options.c_cflag &= ~(CRTSCTS); // no hardware flow control + } + + // At 115k (87 us per char), reading 1 char at a time results in increased + // CPU usage, since we actually can keep up with getting a small number of + // characters per loop. At 921k (11 us per char), we get more characters + // each time through the loop, so there is less advantage to setting VMIN + // to more than 1. + // + // CPU Usage at + // VMIN 115k 921k + // 1 7.0% 1.8% + // 10 2.7% 1.6% + // 100 1.2% 1.2% + // + // The problem with asking for more than 1 character per read is that each + // message will usually not be received until some bytes in the following + // message are available. That is often not a problem, but there are + // sometimes gaps of several 10s of milliseconds in the telemetry stream, + // and it is preferable to process messages as soon as they are available. + if (serialBaud <= 115200) { + options.c_cc[VMIN] = 10; + } else { + options.c_cc[VMIN] = 1; + } + options.c_cc[VTIME] = 0; + + tcsetattr(fd, TCSANOW, &options); // Set the new options for the port "NOW" + + la_log(LOG_INFO, "opened serial port %s", serialPortName.c_str()); + + return; +} + +uint32_t Telem_Serial::handle_read() +{ + ssize_t res = read(fd, _recv_buf, _recv_buflen); + if (res == -1) { + la_log(LOG_INFO, "Failed read: %s", strerror(errno)); + } + + return res; +} + +void Telem_Serial::configure(INIReader *config) +{ + serialPortName = config->Get("solo", "telemDev", "/dev/ttymxc1"); + serialBaud = config->GetInteger("solo", "telemBaud", 57600); + serialFlow = config->GetBoolean("solo", "telemFlow", true); +} + +void Telem_Serial::do_writer_sends() +{ + while (_send_buf_start != _send_buf_stop) { // FIXME: use file descriptors! + bool tail_first = false; + if (_send_buf_stop < _send_buf_start) { + tail_first = true; + } + uint32_t bytes_to_send = + tail_first ? (send_buf_size() - _send_buf_start) : (_send_buf_stop - _send_buf_start); + + int32_t sent = ::write(fd, (const char *)&_send_buf[_send_buf_start], bytes_to_send); + if (sent < 0) { + // cry + break; + } else if (sent == 0) { + break; + } else { + _send_buf_start += sent; + if (_send_buf_start == send_buf_size()) { + _send_buf_start = 0; + } + } + } +} + +bool Telem_Serial::send_message(const mavlink_message_t &msg) +{ + char sendbuf[1024]; // large enough... + + uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t *)sendbuf, &msg); + if (send_buffer_space_free() < messageLen) { + // dropped_packets++; + return false; + } + if (_send_buf_stop >= _send_buf_start) { + uint16_t to_copy = send_buf_size() - _send_buf_stop; + if (to_copy > messageLen) { + to_copy = messageLen; + } + memcpy(&_send_buf[_send_buf_stop], sendbuf, to_copy); + _send_buf_stop += to_copy; + if (_send_buf_stop >= send_buf_size()) { + _send_buf_stop = 0; + } + to_copy = messageLen - to_copy; + if (to_copy) { + memcpy(&_send_buf[_send_buf_stop], &sendbuf[messageLen - to_copy], to_copy); + _send_buf_stop += to_copy; + if (_send_buf_stop >= send_buf_size()) { + _send_buf_stop = 0; + } + } + } else { + memcpy(&_send_buf[_send_buf_stop], &sendbuf[0], messageLen); + _send_buf_stop += messageLen; + if (_send_buf_stop >= send_buf_size()) { + _send_buf_stop = 0; + } + } + return true; +} diff --git a/flightcode/dataflash_logger/telem_serial.h b/flightcode/dataflash_logger/telem_serial.h new file mode 100644 index 0000000..3e73f08 --- /dev/null +++ b/flightcode/dataflash_logger/telem_serial.h @@ -0,0 +1,44 @@ +#include "INIReader.h" + +#include "telem_client.h" + +class Telem_Serial : public Telem_Client +{ +public: + Telem_Serial(uint8_t *recv_buf, uint32_t recv_buflen) : Telem_Client(recv_buf, recv_buflen) + { + } + + uint32_t handle_read(); + + void configure(INIReader *config); + void init() override; + + void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds); + void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds); + + bool send_message(const mavlink_message_t &message); + + void open_serial_port(); + + void do_writer_sends(); + + bool any_data_to_send() + { + return _send_buf_start != _send_buf_stop; + } + +private: + int fd; + + /* send buffer stuff: */ + uint8_t _send_buf[65536]; /* way too big */ + uint32_t send_buf_size() const + { + return 65536; + } + + std::string serialPortName; + uint32_t serialBaud; + bool serialFlow; +}; diff --git a/flightcode/dflog/Makefile b/flightcode/dflog/Makefile new file mode 100644 index 0000000..71f3412 --- /dev/null +++ b/flightcode/dflog/Makefile @@ -0,0 +1,45 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS += -lpthread + +SRCS_CPP = dflog_downloader.cpp +SRCS_C += util.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = dflog + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + $(RM) ../util/*.o + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/dflog/dataFlashMAVLink-to-artoo.py b/flightcode/dflog/dataFlashMAVLink-to-artoo.py new file mode 100755 index 0000000..ebb49ee --- /dev/null +++ b/flightcode/dflog/dataFlashMAVLink-to-artoo.py @@ -0,0 +1,173 @@ +#!/usr/bin/python + +import subprocess +import time +import string +import os +import re + +actually_sync_logs_to_artoo = False + +class RecentLogLinker: + '''RecentLogLinker manages links to the most recent logs. These links are used by the app to retrieve the most recent logs + ''' + def __init__(self, directory,link_dir): + self.directory = directory #location of dataflashlogs + self.symlink_directory = link_dir #loction of links to the dataflashlogs + # if you reduce this number, you're going to have to ensure we + # remove all old stale RECENT links somehow! + self.num_recent_links_to_create = 10 + + # directly copied from LogPruner + def lognum(self, filename): + base = filename.partition('.') + return int(base[0]) + + # directly copied from LogPruner + def ordered_list_of_log_filepaths(self): + binfiles_regex = re.compile("[0-9]+.BIN") + files = [ f for f in os.listdir(self.directory) if (binfiles_regex.match(f) and os.path.isfile(os.path.join(self.directory, f))) ] + files.sort(key=self.lognum) + return [ os.path.join(self.directory, f) for f in files ] + + def update_links(self): + filepaths = self.ordered_list_of_log_filepaths() + #print("Filepaths: " + str(filepaths)) + for i in range(0, self.num_recent_links_to_create): + if len(filepaths) == 0: + break + source = filepaths.pop() + if i == 0: + link_name = "RECENT.BIN" + else: + link_name = "RECENT-{0}.BIN".format(i) + + link_path = os.path.join(self.symlink_directory, link_name) + if os.path.exists(link_path): + existing_source = None + try: + existing_source = os.readlink(link_path) + except OSError as e: + os.unlink(link_path) + if existing_source is not None: + #print("Leaving link alone") + if existing_source == source: + continue + os.unlink(link_path) + + try: + os.symlink(source, link_path) + except OSError as e: + print "Failed to link ({0} to {1}): {2}".format(source, link_path, e.strerror) + break + +class LogPruner: + '''LogPruner removes log files (e.g. 45.BIN) from the supplied + directory until only the suppied number of logs remain. Files are + removed in ascending numerical order + ''' + def __init__(self, directory, max_files_remaining): + self.directory = directory + self.max_files_remaining = max_files_remaining + + # directly copied to RecentLogLinker + def lognum(self, filename): + base = filename.partition('.') + return int(base[0]) + + # directly copied to RecentLogLinker + def ordered_list_of_log_filepaths(self): + binfiles_regex = re.compile("[0-9]+.BIN") + files = [ f for f in os.listdir(self.directory) if (binfiles_regex.match(f) and os.path.isfile(os.path.join(self.directory, f))) ] + files.sort(key=self.lognum) + return [ os.path.join(self.directory, f) for f in files ] + + def run(self): + if not os.path.exists(self.directory): + print("Directory does not exist: {0}".format(self.directory)) + return + ordered_list = self.ordered_list_of_log_filepaths() + while len(ordered_list) > self.max_files_remaining: + to_remove = ordered_list.pop(0) + try: + os.unlink(to_remove) + except IOError as e: + print "Failed to remove ({0}): {1}".format(to_remove, e.strerror) + break + +class LogSyncer: + ''' + an instance of LogSyncer loops over rsync'ing dataflash logs from + the log data directory on solo onto Artoo + ''' + def __init__(self, src, dest, ssh_id_path, rate=100): + self.rsyncSrc = src + self.rsyncDest = dest + self.ssh_id_path = ssh_id_path + self.rate = rate + self.rsyncPartialDest = '.'.join((self.rsyncDest,'part')) + if not os.path.exists(src): + os.makedirs(src) + + def run(self): + # quick sanity checks here. --delete is a very, very big hammer + if len(self.rsyncSrc) < 10: + print("short rsync src") + sys.exit(1) + if len(self.rsyncDest) < 10: + print("short rsync dest") + sys.exit(1) + + recentloglinker = RecentLogLinker(src) + + while True: + recentloglinker.update_links() + + cmd = [ 'ionice', '-c', '3', 'nice', + "rsync", "-a", "--size-only", "--progress", + '-e', 'ssh -o StrictHostKeyChecking=no -i "%s"' % self.ssh_id_path, + "--partial", + "--delete", + '--bwlimit=%s' % (self.rate,), + self.rsyncSrc, self.rsyncDest] +# print "cmd: %s" % (' '.join(cmd)) + p = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + stdout,stderr = p.communicate() + rc = p.returncode + if rc != 0: + print "stderr: (%s)" % stderr + time.sleep(1); + continue + +# print "stdout: (%s)" % stdout + + timeout = 10 + if string.find(stdout,"xfr") != -1: + timeout = 1 + time.sleep(timeout); + +# TODO: get artoo's address from config +# TODO: get source and dest addresses from config +src = "/log/dataflash/" +dest = "root@10.1.1.1:/log/solo/dataflash" +link_dir = "/log/" #location of symlinks for app to retrieve +ssh_id_path = "/home/root/.ssh/id_rsa-mav-df-xfer" +max_log_files=50 + +p = LogPruner(src, max_log_files) +p.run() + +if actually_sync_logs_to_artoo: + l = LogSyncer(src, dest, ssh_id_path) + l.run() +else: + # we still update the links on SoloLink so the app can fetch them + recentloglinker = RecentLogLinker(src,link_dir) + + while True: + recentloglinker.update_links() + time.sleep(10); + +# neither reached nor necessary if the LogSyncer runs +while True: + time.sleep(86400) diff --git a/flightcode/dflog/dflog_downloader.cpp b/flightcode/dflog/dflog_downloader.cpp new file mode 100644 index 0000000..ade563c --- /dev/null +++ b/flightcode/dflog/dflog_downloader.cpp @@ -0,0 +1,497 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "../mavlink/c_library/common/mavlink.h" +#include +#include +#include +#include +#include + +using namespace std; + +/*********************************************************************** +Serial port name +***********************************************************************/ +string serialPortName; + +/*********************************************************************** +File descriptors +***********************************************************************/ +int serial_fd; + +/*********************************************************************** +Threading variables +***********************************************************************/ +pthread_mutex_t mutex_msg; +pthread_mutex_t mutex_msgtype; +mavlink_message_t latest_msg; +sem_t sem_msg; +int msg_type = 0; + +/*********************************************************************** +Log file variables +***********************************************************************/ +int log_size; +ofstream logfile; +int log_num = 0; + +/*********************************************************************** +User input variables +***********************************************************************/ +bool get_latest = false; + +/*********************************************************************** +Serial tx/rx buffer size +***********************************************************************/ +#define BUFSIZE 256 + +/*********************************************************************** +Pointer to log buffer. Allocate a really big buffer later +that is the size of the log file. This is probably not the best way +to go about this, but it works in a pinch. +***********************************************************************/ +char *logbuf; + +/*********************************************************************** +Function: int serial_setup(int baud) + +Description: The serial port initialization function. This function + initializes the serial port over which DSM data is sent to + the pixhawk. A return of 0 indicates an error. +***********************************************************************/ +int serial_setup(int baud) +{ + struct termios options; + + serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY); + + if (serial_fd < 0) { + cerr << "Unable to open serial port " << serialPortName.c_str() << endl; + return 0; + } + + tcflush(serial_fd, TCIOFLUSH); + + // Configure port for 8N1 transmission + tcgetattr(serial_fd, &options); // Gets the current options for the port + // Set the output baud rate + switch (baud) { + case 57600: + cfsetspeed(&options, B57600); + break; + case 115200: + cfsetspeed(&options, B115200); + break; + case 1500000: + cfsetspeed(&options, B1500000); + break; + default: + cerr << "Unsupported baud rate" << endl; + return 0; + } + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; + options.c_cflag |= CRTSCTS; + options.c_cc[VMIN] = 17; + options.c_cc[VTIME] = 0; + + tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW" + + sleep(1); + tcflush(serial_fd, TCIOFLUSH); + + cout << "Opened serial port " << serialPortName.c_str() << endl; + + return 1; +} + +/*********************************************************************** +Function: int print_pct() + +Description: Prints a pct bar on the console and displays the + current download speed. +***********************************************************************/ +void print_pct(float pct, float speed) +{ + int maxWidth = 50; + int pos; + int i; + + cout << "["; + pos = maxWidth * pct; + for (i = 0; i < maxWidth; ++i) { + if (i < pos) + cout << "="; + else if (i == pos) + cout << ">"; + else + cout << " "; + } + cout << "] " << int(pct * 100.0) << "%"; + cout << " " << speed << " kB/s \r"; + cout << flush; +} + +/*********************************************************************** +Function: int handle_log_data() + +Description: Take log data and dump it into a log file +***********************************************************************/ +int handle_log_data(mavlink_message_t *msg) +{ + int ofs, cnt, id; + uint64_t now_us; + float speed = 0.0; + char logdat[128]; + + static int fileofs = 0; + static int disc_cnt = 0; + static uint64_t last_us = 0; + static int bytes_received = 0; + static int last_bytes_received = 0; + static char *buf_ptr = logbuf; + + id = mavlink_msg_log_data_get_id(msg); + + if (id != log_num) { + cout << "Got data from a different log, ignoring" << endl; + return 0; + } + + ofs = mavlink_msg_log_data_get_ofs(msg); + cnt = mavlink_msg_log_data_get_count(msg); + mavlink_msg_log_data_get_data(msg, (uint8_t *)logdat); + + // I don't think this ever happens, but if we get a zero + // count then we're done + if (cnt == 0) { + cout << endl; + cout << "Log download complete" << endl; + return 1; + } + + // Write data directly to the (really big) buffer + memcpy(buf_ptr, logdat, cnt); + buf_ptr += cnt; + + if (fileofs != ofs) + disc_cnt++; + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + bytes_received += cnt; + + // Output + if ((now_us - last_us) > 1e6) { + speed = + float(bytes_received - last_bytes_received) / 1024. / float((now_us - last_us) / 1e6); + + print_pct(float(bytes_received) / float(log_size), speed); + + last_us = now_us; + last_bytes_received = bytes_received; + } + + // Update the file position + fileofs = ofs + cnt; + + // Are we done yet? + if (fileofs == log_size) { + print_pct(1, speed); + cout << endl; + cout << "Log download complete." << endl; + cout << "Downloaded " << float(bytes_received) / 1024. << "kB, dropped " << disc_cnt + << " packet(s)" << endl; + return 1; + } + + return 0; +} + +/*********************************************************************** +Function: int mavlink_task() + +Description: Thread task to pull mavlink data from the serial port + and dump it into a mutex-protected mavlink message. + + When log data begins streaming, dumps log data into the + log file on the filesystem. +***********************************************************************/ +void *mavlink_task(void *) +{ + mavlink_message_t _msg; + mavlink_status_t mavlink_status; + int read_len; + int i; + char buf[BUFSIZE]; + int msgtolookfor; + + while (1) { + read_len = read(serial_fd, buf, BUFSIZE); + + if (read_len < 0) { + if (errno != EAGAIN) + cerr << "Read err: " << errno << endl; + } + + if (read_len > 0) { + for (i = 0; i < read_len; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &_msg, &mavlink_status)) { + // If this is the type of message we're waiting for, put it on + // the latest_msg or handle its log data. + pthread_mutex_lock(&mutex_msgtype); + msgtolookfor = msg_type; + pthread_mutex_unlock(&mutex_msgtype); + + if (_msg.msgid == msgtolookfor) { + if (_msg.msgid == MAVLINK_MSG_ID_LOG_DATA) { + if (handle_log_data(&_msg)) { + cout << "Writing to logfile..."; + logfile.write(logbuf, log_size); + system("sync"); + cout << "Done!" << endl; + pthread_exit(NULL); + } + } else { + pthread_mutex_lock(&mutex_msg); + memcpy(&latest_msg, &_msg, sizeof(mavlink_message_t)); + pthread_mutex_unlock(&mutex_msg); + sem_post(&sem_msg); + } + } + } + } + } + } + pthread_exit(NULL); +} + +/*********************************************************************** +Function: int wait_for_message() + +Description: Blocks until a particular mavlink message type is received. + A return value of 0 indicates succes, -1 for a timeout. + Setting timeout_s to 0 blocks indefinitely. +***********************************************************************/ +int wait_for_message(int _msg_type, mavlink_message_t *msg, int timeout_s) +{ + struct timespec timeout; + int ret; + + // Tell the mavlink thread which message to look for + pthread_mutex_lock(&mutex_msgtype); + msg_type = _msg_type; + pthread_mutex_unlock(&mutex_msgtype); + + clock_gettime(CLOCK_REALTIME, &timeout); + timeout.tv_sec += timeout_s; + + if (timeout_s == 0) + sem_wait(&sem_msg); + else { + ret = sem_timedwait(&sem_msg, &timeout); + + if (ret < 0) { + if (errno == ETIMEDOUT) + return -1; + } + } + + pthread_mutex_lock(&mutex_msg); + memcpy(msg, &latest_msg, sizeof(mavlink_message_t)); + pthread_mutex_unlock(&mutex_msg); + + return 0; +} + +/*********************************************************************** +Function: void request_log() + +Description: Request a particular log file from the pixhawk. +***********************************************************************/ +void request_log(void) +{ + char buf[BUFSIZE]; + mavlink_message_t msg; + int msg_len; + char logfilename[] = "/log/log12345.bin"; + + // Wait for a heartbeat + if (wait_for_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 5) < 0) { + cout << "No heartbeat received" << endl; + exit(1); + } + + // Get the latest log number (num_logs works, last_log_num doesn't) + if (get_latest) { + mavlink_msg_log_request_list_pack(1, 1, &msg, 1, 1, log_num, log_num); + msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg); + + if (write(serial_fd, buf, msg_len) != msg_len) { + cerr << "Serial port write error." << endl; + exit(1); + } + + cout << "Requesting latest log number..."; + + if (wait_for_message(MAVLINK_MSG_ID_LOG_ENTRY, &msg, 10) < 0) { + cout << "Unable to get log information" << endl; + exit(1); + } + + log_num = mavlink_msg_log_entry_get_num_logs(&msg); + cout << "Latest log is " << log_num << endl; + } + + // Get some size information about the log we want. + mavlink_msg_log_request_list_pack(1, 1, &msg, 1, 1, log_num, log_num); + msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg); + if (write(serial_fd, buf, msg_len) != msg_len) { + cerr << "Serial port write error." << endl; + exit(1); + } + + cout << "Waiting for log information..." << endl; + + if (wait_for_message(MAVLINK_MSG_ID_LOG_ENTRY, &msg, 10) < 0) { + cout << "Unable to get log information" << endl; + exit(1); + } + + log_size = mavlink_msg_log_entry_get_size(&msg); + if (log_size == 0) { + cout << "Log " << log_num << " does not exist." << endl; + exit(1); + } else { + cout << "Log size is " << float(log_size) / 1024. << "kB" << endl; + + logbuf = new char[log_size]; + if (logbuf == NULL) { + cout << "Unable to allocate buffer for log" << endl; + exit(1); + } + } + + // Set up the log file storage + sprintf(logfilename, "/log/log%i.bin", log_num); + logfile.open(logfilename, ios::out | ios::binary); + + // Request log data + cout << "Beginning log download..." << endl; + mavlink_msg_log_request_data_pack(1, 1, &msg, 1, 1, log_num, 0, 0xFFFFFFFF); + msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg); + + // Let the thread know we've requested data. + pthread_mutex_lock(&mutex_msgtype); + msg_type = MAVLINK_MSG_ID_LOG_DATA; + pthread_mutex_unlock(&mutex_msgtype); + + if (write(serial_fd, buf, msg_len) != msg_len) { + cerr << "Serial port write error." << endl; + exit(1); + } + + // Let the mavlink thread do its thing +} + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(int argc, char *argv[]) +{ + int baudrate; + pthread_t mavlink_ctx; + char usbdev_string[256] = "usb-3D_Robotics_PX4_FMU"; + DIR *dir; + struct dirent *direntry; + bool foundDevice = false; + + if (argc < 2) { + cout << "Usage: dflog " << endl; + return -1; + } + + if (strcmp(argv[1], "latest") == 0) { + cout << "Pulling latest log" << endl; + get_latest = true; + } else { + log_num = atoi(argv[1]); + cout << "Pulling log " << log_num << endl; + } + + // Cout cleanup + std::cout.precision(2); + std::cout.setf(ios::fixed, ios::floatfield); + + dir = opendir("/dev/serial/by-id/"); + if (dir == NULL) { + cerr << "open /dev/serial/by-id failed" << endl; + return -1; + } + + while ((direntry = readdir(dir))) { + if (!strncmp(direntry->d_name, usbdev_string, 23)) { + foundDevice = true; + strcpy(usbdev_string, direntry->d_name); + break; + } + } + + if (!foundDevice) { + cerr << "Unable to find USB device" << endl; + return -1; + } + + closedir(dir); + + // Serial port setup + serialPortName = "/dev/serial/by-id/"; + serialPortName.append(usbdev_string); + baudrate = 115200; + if (!serial_setup(baudrate)) { + cerr << "Unable to initialize the serial send" << endl; + return -1; + } + + // Threading initialization + pthread_mutex_init(&mutex_msg, NULL); + sem_init(&sem_msg, 0, 0); + + // Start the mavlink rx thread + pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL); + + // Send a log request to the pixhawk + request_log(); + + // Wait for the mavlink thread to end + pthread_join(mavlink_ctx, NULL); + + logfile.close(); + + return 0; +} diff --git a/flightcode/dflog/loadLog.py b/flightcode/dflog/loadLog.py new file mode 100755 index 0000000..037d3db --- /dev/null +++ b/flightcode/dflog/loadLog.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python + +import subprocess +import sys +import os +import time +from pymavlink import mavutil +import glob +import ConfigParser +import shutil +from datetime import datetime +import argparse + +SELECT_GPIO = "21" +ENABLE_GPIO = "19" + +#GPIO direction set +def setGPIODir(gpio, direction): + dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w") + dir_fd.write(direction) + dir_fd.close() + +#Open the GPIO +def openGPIO(gpio): + #Check and see if the GPIO is already exported + if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)): + #otherwise export it + exp_fd = open("/sys/class/gpio/export", "w") + exp_fd.write(gpio) + exp_fd.close() + + setGPIODir(gpio, "out"); + +def closeGPIO(gpio): + unexp_fd = open("/sys/class/gpio/unexport", "w") + unexp_fd.write(gpio) + unexp_fd.close() + +def setGPIO(gpio, value): + val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w") + val_fd.write(value) + val_fd.close() + +def openSetClose(gpio, value): + openGPIO(gpio) + setGPIO(gpio, value) + closeGPIO(gpio) + +#Set the GPIO low +def disconnectAndExit(): + openSetClose(SELECT_GPIO, "0") + openSetClose(ENABLE_GPIO, "1") + sys.exit() + +parser = argparse.ArgumentParser() +parser.add_argument("lognum", help="Log number to download, or 'latest'") +args = parser.parse_args() + +#Log downloading process +print "Pixhawk log loader" + +#Set the USB select GPIOs +openSetClose(SELECT_GPIO, "1") +openSetClose(ENABLE_GPIO, "0") +time.sleep(1) + +print "Checking for pixhawk on USB" + +usb_devs = glob.glob('/dev/serial/by-id/usb-3D*') +if not usb_devs: + print "No pixhawk found on USB. Exiting." + disconnectAndExit() + +print "Pixhawk found on USB, requesting log." +pixhawk_usb = usb_devs[-1] +m = mavutil.mavlink_connection(pixhawk_usb) + +#Call the log downloader app +ret = subprocess.call(["dflog", str(args.lognum)]) + +disconnectAndExit() + diff --git a/flightcode/hostapd_ctrl/Makefile b/flightcode/hostapd_ctrl/Makefile new file mode 100644 index 0000000..78544ad --- /dev/null +++ b/flightcode/hostapd_ctrl/Makefile @@ -0,0 +1,40 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) + +SRCS_C = main.c hostapd_ctrl.c util.c + +OBJS = $(SRCS_C:.c=.o) + +MAIN = hostapd_ctrl + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.c) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/hostapd_ctrl/main.c b/flightcode/hostapd_ctrl/main.c new file mode 100644 index 0000000..b6a3948 --- /dev/null +++ b/flightcode/hostapd_ctrl/main.c @@ -0,0 +1,48 @@ + +#include +#include +#include +#include +#include "util.h" +#include "hostapd_ctrl.h" + +/* Test program for hostapd_ctrl */ + +/* make this small (e.g. 2) to test the table-too-small case */ +#define STATIONS_MAX 10 + +int main(int argc, char *argv[]) +{ + void *handle; + hostapd_station_info_t station_info[STATIONS_MAX]; + int stations; + int i; + char mac_string[MAC_STRING_LEN]; + + handle = hostapd_ctrl_new("wlan0-ap"); + if (handle == NULL) { + printf("ERROR creating control connection\n"); + exit(1); + } + + stations = STATIONS_MAX; + if (hostapd_ctrl_get_stations(handle, station_info, &stations) != 0) { + printf("ERROR getting station info\n"); + hostapd_ctrl_delete(handle); + exit(1); + } + + printf("%d stations:\n", stations); + + if (stations > STATIONS_MAX) + /* there are more stations than would fit in the table we supplied */ + stations = STATIONS_MAX; + + for (i = 0; i < stations; i++) + printf("%s\n", mac_ntoa(station_info[i].mac, mac_string)); + + hostapd_ctrl_delete(handle); + + exit(0); + +} /* main */ diff --git a/flightcode/ini/Makefile b/flightcode/ini/Makefile new file mode 100644 index 0000000..53a8a7f --- /dev/null +++ b/flightcode/ini/Makefile @@ -0,0 +1,14 @@ + +all: + +clean: + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/ini/cpp/INIReader.cpp b/flightcode/ini/cpp/INIReader.cpp new file mode 100644 index 0000000..d8507db --- /dev/null +++ b/flightcode/ini/cpp/INIReader.cpp @@ -0,0 +1,75 @@ +// Read an INI file into easy-to-access name/value pairs. + +#include +#include +#include +#include "../ini.h" +#include "INIReader.h" + +using std::string; + +INIReader::INIReader(string filename) +{ + _error = ini_parse(filename.c_str(), ValueHandler, this); +} + +int INIReader::ParseError() +{ + return _error; +} + +string INIReader::Get(string section, string name, string default_value) +{ + string key = MakeKey(section, name); + return _values.count(key) ? _values[key] : default_value; +} + +long INIReader::GetInteger(string section, string name, long default_value) +{ + string valstr = Get(section, name, ""); + const char *value = valstr.c_str(); + char *end; + // This parses "1234" (decimal) and also "0x4D2" (hex) + long n = strtol(value, &end, 0); + return end > value ? n : default_value; +} + +double INIReader::GetReal(string section, string name, double default_value) +{ + string valstr = Get(section, name, ""); + const char *value = valstr.c_str(); + char *end; + double n = strtod(value, &end); + return end > value ? n : default_value; +} + +bool INIReader::GetBoolean(string section, string name, bool default_value) +{ + string valstr = Get(section, name, ""); + // Convert to lower case to make string comparisons case-insensitive + std::transform(valstr.begin(), valstr.end(), valstr.begin(), ::tolower); + if (valstr == "true" || valstr == "yes" || valstr == "on" || valstr == "1") + return true; + else if (valstr == "false" || valstr == "no" || valstr == "off" || valstr == "0") + return false; + else + return default_value; +} + +string INIReader::MakeKey(string section, string name) +{ + string key = section + "." + name; + // Convert to lower case to make section/name lookups case-insensitive + std::transform(key.begin(), key.end(), key.begin(), ::tolower); + return key; +} + +int INIReader::ValueHandler(void *user, const char *section, const char *name, const char *value) +{ + INIReader *reader = (INIReader *)user; + string key = MakeKey(section, name); + if (reader->_values[key].size() > 0) + reader->_values[key] += "\n"; + reader->_values[key] += value; + return 1; +} diff --git a/flightcode/ini/cpp/INIReader.h b/flightcode/ini/cpp/INIReader.h new file mode 100644 index 0000000..dc80936 --- /dev/null +++ b/flightcode/ini/cpp/INIReader.h @@ -0,0 +1,51 @@ +// Read an INI file into easy-to-access name/value pairs. + +// inih and INIReader are released under the New BSD license (see LICENSE.txt). +// Go to the project home page for more info: +// +// http://code.google.com/p/inih/ + +#ifndef __INIREADER_H__ +#define __INIREADER_H__ + +#include +#include + +// Read an INI file into easy-to-access name/value pairs. (Note that I've gone +// for simplicity here rather than speed, but it should be pretty decent.) +class INIReader +{ +public: + // Construct INIReader and parse given filename. See ini.h for more info + // about the parsing. + INIReader(std::string filename); + + // Return the result of ini_parse(), i.e., 0 on success, line number of + // first error on parse error, or -1 on file open error. + int ParseError(); + + // Get a string value from INI file, returning default_value if not found. + std::string Get(std::string section, std::string name, std::string default_value); + + // Get an integer (long) value from INI file, returning default_value if + // not found or not a valid integer (decimal "1234", "-1234", or hex "0x4d2"). + long GetInteger(std::string section, std::string name, long default_value); + + // Get a real (floating point double) value from INI file, returning + // default_value if not found or not a valid floating point value + // according to strtod(). + double GetReal(std::string section, std::string name, double default_value); + + // Get a boolean value from INI file, returning default_value if not found or if + // not a valid true/false value. Valid true values are "true", "yes", "on", "1", + // and valid false values are "false", "no", "off", "0" (not case sensitive). + bool GetBoolean(std::string section, std::string name, bool default_value); + +private: + int _error; + std::map< std::string, std::string > _values; + static std::string MakeKey(std::string section, std::string name); + static int ValueHandler(void *user, const char *section, const char *name, const char *value); +}; + +#endif // __INIREADER_H__ diff --git a/flightcode/ini/cpp/Makefile b/flightcode/ini/cpp/Makefile new file mode 100644 index 0000000..478d1ef --- /dev/null +++ b/flightcode/ini/cpp/Makefile @@ -0,0 +1,14 @@ + +all: + +clean: + +BASE := ../../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/ini/ini.c b/flightcode/ini/ini.c new file mode 100644 index 0000000..d231fc3 --- /dev/null +++ b/flightcode/ini/ini.c @@ -0,0 +1,174 @@ +/* inih -- simple .INI file parser + +inih is released under the New BSD license (see LICENSE.txt). Go to the project +home page for more info: + +http://code.google.com/p/inih/ + +*/ + +#include +#include +#include + +#include "ini.h" + +#if !INI_USE_STACK +#include +#endif + +#define MAX_SECTION 50 +#define MAX_NAME 50 + +/* Strip whitespace chars off end of given string, in place. Return s. */ +static char *rstrip(char *s) +{ + char *p = s + strlen(s); + while (p > s && isspace((unsigned char)(*--p))) + *p = '\0'; + return s; +} + +/* Return pointer to first non-whitespace char in given string. */ +static char *lskip(const char *s) +{ + while (*s && isspace((unsigned char)(*s))) + s++; + return (char *)s; +} + +/* Return pointer to first char c or ';' comment in given string, or pointer to + null at end of string if neither found. ';' must be prefixed by a whitespace + character to register as a comment. */ +static char *find_char_or_comment(const char *s, char c) +{ + int was_whitespace = 0; + while (*s && *s != c && !(was_whitespace && *s == ';')) { + was_whitespace = isspace((unsigned char)(*s)); + s++; + } + return (char *)s; +} + +/* Version of strncpy that ensures dest (size bytes) is null-terminated. */ +static char *strncpy0(char *dest, const char *src, size_t size) +{ + strncpy(dest, src, size); + dest[size - 1] = '\0'; + return dest; +} + +/* See documentation in header file. */ +int ini_parse_file(FILE *file, int (*handler)(void *, const char *, const char *, const char *), + void *user) +{ +/* Uses a fair bit of stack (use heap instead if you need to) */ +#if INI_USE_STACK + char line[INI_MAX_LINE]; +#else + char *line; +#endif + char section[MAX_SECTION] = ""; + char prev_name[MAX_NAME] = ""; + + char *start; + char *end; + char *name; + char *value; + int lineno = 0; + int error = 0; + +#if !INI_USE_STACK + line = (char *)malloc(INI_MAX_LINE); + if (!line) { + return -2; + } +#endif + + /* Scan through file line by line */ + while (fgets(line, INI_MAX_LINE, file) != NULL) { + lineno++; + + start = line; +#if INI_ALLOW_BOM + if (lineno == 1 && (unsigned char)start[0] == 0xEF && (unsigned char)start[1] == 0xBB && + (unsigned char)start[2] == 0xBF) { + start += 3; + } +#endif + start = lskip(rstrip(start)); + + if (*start == ';' || *start == '#') { + /* Per Python ConfigParser, allow '#' comments at start of line */ + } +#if INI_ALLOW_MULTILINE + else if (*prev_name && *start && start > line) { + /* Non-black line with leading whitespace, treat as continuation + of previous name's value (as per Python ConfigParser). */ + if (!handler(user, section, prev_name, start) && !error) + error = lineno; + } +#endif + else if (*start == '[') { + /* A "[section]" line */ + end = find_char_or_comment(start + 1, ']'); + if (*end == ']') { + *end = '\0'; + strncpy0(section, start + 1, sizeof(section)); + *prev_name = '\0'; + } else if (!error) { + /* No ']' found on section line */ + error = lineno; + } + } else if (*start && *start != ';') { + /* Not a comment, must be a name[=:]value pair */ + end = find_char_or_comment(start, '='); + if (*end != '=') { + end = find_char_or_comment(start, ':'); + } + if (*end == '=' || *end == ':') { + *end = '\0'; + name = rstrip(start); + value = lskip(end + 1); + end = find_char_or_comment(value, '\0'); + if (*end == ';') + *end = '\0'; + rstrip(value); + + /* Valid name[=:]value pair found, call handler */ + strncpy0(prev_name, name, sizeof(prev_name)); + if (!handler(user, section, name, value) && !error) + error = lineno; + } else if (!error) { + /* No '=' or ':' found on name[=:]value line */ + error = lineno; + } + } + +#if INI_STOP_ON_FIRST_ERROR + if (error) + break; +#endif + } + +#if !INI_USE_STACK + free(line); +#endif + + return error; +} + +/* See documentation in header file. */ +int ini_parse(const char *filename, + int (*handler)(void *, const char *, const char *, const char *), void *user) +{ + FILE *file; + int error; + + file = fopen(filename, "r"); + if (!file) + return -1; + error = ini_parse_file(file, handler, user); + fclose(file); + return error; +} diff --git a/flightcode/ini/ini.h b/flightcode/ini/ini.h new file mode 100644 index 0000000..cd481d6 --- /dev/null +++ b/flightcode/ini/ini.h @@ -0,0 +1,75 @@ +/* inih -- simple .INI file parser + +inih is released under the New BSD license (see LICENSE.txt). Go to the project +home page for more info: + +http://code.google.com/p/inih/ + +*/ + +#ifndef __INI_H__ +#define __INI_H__ + +/* Make this header file easier to include in C++ code */ +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* Parse given INI-style file. May have [section]s, name=value pairs + (whitespace stripped), and comments starting with ';' (semicolon). Section + is "" if name=value pair parsed before any section heading. name:value + pairs are also supported as a concession to Python's ConfigParser. + + For each name=value pair parsed, call handler function with given user + pointer as well as section, name, and value (data only valid for duration + of handler call). Handler should return nonzero on success, zero on error. + + Returns 0 on success, line number of first error on parse error (doesn't + stop on first error), -1 on file open error, or -2 on memory allocation + error (only when INI_USE_STACK is zero). +*/ +int ini_parse(const char *filename, + int (*handler)(void *user, const char *section, const char *name, const char *value), + void *user); + +/* Same as ini_parse(), but takes a FILE* instead of filename. This doesn't + close the file when it's finished -- the caller must do that. */ +int ini_parse_file(FILE *file, int (*handler)(void *user, const char *section, const char *name, + const char *value), + void *user); + +/* Nonzero to allow multi-line value parsing, in the style of Python's + ConfigParser. If allowed, ini_parse() will call the handler with the same + name for each subsequent line parsed. */ +#ifndef INI_ALLOW_MULTILINE +#define INI_ALLOW_MULTILINE 1 +#endif + +/* Nonzero to allow a UTF-8 BOM sequence (0xEF 0xBB 0xBF) at the start of + the file. See http://code.google.com/p/inih/issues/detail?id=21 */ +#ifndef INI_ALLOW_BOM +#define INI_ALLOW_BOM 1 +#endif + +/* Nonzero to use stack, zero to use heap (malloc/free). */ +#ifndef INI_USE_STACK +#define INI_USE_STACK 1 +#endif + +/* Stop parsing on first error (default is to keep parsing). */ +#ifndef INI_STOP_ON_FIRST_ERROR +#define INI_STOP_ON_FIRST_ERROR 0 +#endif + +/* Maximum line length for any line in INI file. */ +#ifndef INI_MAX_LINE +#define INI_MAX_LINE 200 +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __INI_H__ */ diff --git a/flightcode/log/Log.cpp b/flightcode/log/Log.cpp new file mode 100644 index 0000000..e60a6e6 --- /dev/null +++ b/flightcode/log/Log.cpp @@ -0,0 +1,160 @@ +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "Log.h" + +using namespace std; + +/********************************************************************** +Function: Log() + +Description: The log constructor. Opens an fstream file based on + the filename string. Sets the maximum size of the log file + and the maximum number of log files to be written. +***********************************************************************/ +Log::Log(string filename, long int maxFileSize, int maxLogFiles) + : _maxFileSize(maxFileSize), _maxLogFiles(maxLogFiles), _logFilename(filename) +{ + log_fd.precision(2); + log_fd.setf(ios::fixed, ios::floatfield); + + // Open the log file + log_fd.open(filename.c_str(), ios::out | std::ofstream::app); + if (!log_fd) { + cerr << "Could not open log file" << endl; + return; + } +} + +/********************************************************************** +Function: Log() + +Description: Overloaded log constructor with an option for creating a + new log when the log starts up +***********************************************************************/ +Log::Log(string filename, long int maxFileSize, int maxLogFiles, bool newLogOnBoot) + : _maxFileSize(maxFileSize), _maxLogFiles(maxLogFiles), _logFilename(filename) +{ + log_fd.precision(2); + log_fd.setf(ios::fixed, ios::floatfield); + bool fileExists = doesFileExist(filename); + + // Open the log file + log_fd.open(filename.c_str(), ios::out | std::ofstream::app); + if (!log_fd) { + cerr << "Could not open log file" << endl; + return; + } + + if (newLogOnBoot && fileExists) + forceRoll(); +} + +/********************************************************************** +Function: getFilesize() + +Description: Gets the filesize of a file named by filename +***********************************************************************/ +long int Log::getFilesize(string filename) +{ + struct stat filestatus; + + stat(filename.c_str(), &filestatus); + + return filestatus.st_size; +} + +/********************************************************************** +Function: doesFileExist() + +Description: Returns a bool indicating if the file named filename + exists. filename must be an absolute path. +***********************************************************************/ +bool Log::doesFileExist(string filename) +{ + struct stat filestatus; + + return (stat(filename.c_str(), &filestatus) == 0); +} + +/*********************************************************************** +Function: char* getTimeString(void) + +Description: Gets the current time string. + ***********************************************************************/ +const char *Log::getTimeString(void) +{ + static char buffer[80]; + + clock_gettime_str_r(CLOCK_REALTIME, buffer); + + return buffer; +} + +/********************************************************************** +Function: checkSizeAndRoll() + +Description: Checks to see if the log file has grown too big and should + be rolled over. Rolls it and all previous logfiles + if necessary. The calling application should call this + function periodically after writing to the logfile. +***********************************************************************/ +void Log::checkSizeAndRoll(void) +{ + stringstream filename, newfilename; + + // Check the size of the file. If its too big, roll the logfiles. + if (getFilesize(_logFilename) > _maxFileSize) { + // Roll the log files up to the max-1 + for (int i = 1; i < _maxLogFiles; ++i) { + filename.str(string()); + filename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i); + + // See if the file exists + if (doesFileExist(filename.str())) { + newfilename.str(string()); + newfilename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i + 1); + + rename(filename.str().c_str(), newfilename.str().c_str()); + } + } + + // Roll the current file + log_fd.close(); + rename(_logFilename.c_str(), filename.str().c_str()); + log_fd.open(_logFilename.c_str(), ios::out | std::ofstream::app); + } +} + +/********************************************************************** +Function: forceRoll() + +Description: Forces a file roll +***********************************************************************/ +void Log::forceRoll(void) +{ + stringstream filename, newfilename; + + // Roll the log files up to the max-1 + for (int i = 1; i < _maxLogFiles; ++i) { + filename.str(string()); + filename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i); + + // See if the file exists + if (doesFileExist(filename.str())) { + newfilename.str(string()); + newfilename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i + 1); + + rename(filename.str().c_str(), newfilename.str().c_str()); + } + } + + // Roll the current file + log_fd.close(); + rename(_logFilename.c_str(), filename.str().c_str()); + log_fd.open(_logFilename.c_str(), ios::out | std::ofstream::app); +} diff --git a/flightcode/log/Log.h b/flightcode/log/Log.h new file mode 100644 index 0000000..06be426 --- /dev/null +++ b/flightcode/log/Log.h @@ -0,0 +1,41 @@ +#ifndef _LOG_H +#define _LOG_H + +#include +#include +#include + +using namespace std; + +/********************************************************************** +Class: Log + +Description: A simple logging class. Opens a logfile specified by the + calling function. The log file can be rolled when its + size gets large enough, out to maxLogFiles files. +***********************************************************************/ +class Log +{ +public: + Log(string filename, long int maxFileSize, int maxLogFiles); + Log(string filename, long int maxFileSize, int maxLogFiles, bool newLogOnBoot); + void checkSizeAndRoll(void); + void forceRoll(void); + + // Left as a public for use in logging macros + const char *getTimeString(void); + + // Leave the file descriptor public so it can be written to + // by the calling application. + ofstream log_fd; + +private: + long int _maxFileSize; + int _maxLogFiles; + string _logFilename; + + bool doesFileExist(string filename); + long int getFilesize(string filename); +}; + +#endif //_LOG_H diff --git a/flightcode/log/Makefile b/flightcode/log/Makefile new file mode 100644 index 0000000..53a8a7f --- /dev/null +++ b/flightcode/log/Makefile @@ -0,0 +1,14 @@ + +all: + +clean: + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/mavlink/c_library/ardupilotmega/ardupilotmega.h b/flightcode/mavlink/c_library/ardupilotmega/ardupilotmega.h new file mode 100644 index 0000000..cc5d8ae --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/ardupilotmega.h @@ -0,0 +1,708 @@ +/** @file + * @brief MAVLink comm protocol generated from ardupilotmega.xml + * @see http://mavlink.org + */ +#ifndef MAVLINK_ARDUPILOTMEGA_H +#define MAVLINK_ARDUPILOTMEGA_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_ARDUPILOTMEGA.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 229, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 12, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 0, 0, 0, 0, 0, 0, 36, 60, 14, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 45, 4, 40, 2, 206, 7, 29, 0, 0, 0, 0, 27, 44, 22, 25, 0, 0, 0, 0, 0, 42, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 3, 3, 6, 7, 2, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 254, 36, 30, 18, 18, 51, 9, 0} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 59, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 0, 0, 0, 0, 0, 0, 154, 178, 255, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 189, 52, 174, 229, 85, 159, 186, 72, 0, 0, 0, 0, 92, 36, 71, 98, 0, 0, 0, 0, 0, 134, 205, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 69, 101, 50, 202, 17, 162, 0, 0, 0, 0, 0, 76, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 204, 49, 170, 44, 83, 46, 0} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, MAVLINK_MESSAGE_INFO_BATTERY2, MAVLINK_MESSAGE_INFO_AHRS3, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION_REQUEST, MAVLINK_MESSAGE_INFO_REMOTE_LOG_DATA_BLOCK, MAVLINK_MESSAGE_INFO_REMOTE_LOG_BLOCK_STATUS, MAVLINK_MESSAGE_INFO_LED_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MAG_CAL_PROGRESS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, MAVLINK_MESSAGE_INFO_EKF_STATUS_REPORT, MAVLINK_MESSAGE_INFO_PID_TUNING, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GIMBAL_REPORT, MAVLINK_MESSAGE_INFO_GIMBAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GIMBAL_TORQUE_CMD_REPORT, MAVLINK_MESSAGE_INFO_GOPRO_HEARTBEAT, MAVLINK_MESSAGE_INFO_GOPRO_GET_REQUEST, MAVLINK_MESSAGE_INFO_GOPRO_GET_RESPONSE, MAVLINK_MESSAGE_INFO_GOPRO_SET_REQUEST, MAVLINK_MESSAGE_INFO_GOPRO_SET_RESPONSE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GPS_ACCURACY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_ARDUPILOTMEGA + +// ENUM DEFINITIONS + + +/** @brief */ +#ifndef HAVE_ENUM_MAV_CMD +#define HAVE_ENUM_MAV_CMD +typedef enum MAV_CMD +{ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT=30, /* Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. |Empty| Empty| Empty| Empty| Empty| Empty| Desired altitude in meters| */ + MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Heading Required (0 = False)| Radius in meters. If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ + MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude (meters)| Landing speed (m/s)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test |motor sequence number (a number from 1 to max number of motors on the vehicle)| throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)| throttle| timeout (in seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GRIPPER=211, /* Mission command to operate EPM gripper |gripper number (a number from 1 to max number of grippers on the vehicle)| gripper action (0=release, 1=grab. See GRIPPER_ACTIONS enum)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: start logging with rate of param 3 in Hz (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities |1: Request autopilot version| Reserved (all remaining params)| */ + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence |Duration between two consecutive pictures (in seconds)| Number of images to capture total - 0 for unlimited capture| Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc)| */ + MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence |Reserved| Reserved| */ + MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start)| Shutter integration time (in ms)| Reserved| */ + MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture |Camera ID (0 for all cameras), 1 for first, 2 for second, etc.| Frames per second| Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc)| */ + MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture |Reserved| Reserved| */ + MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */ + MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overriden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters AMSL| */ + MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_POWER_OFF_INITIATED=42000, /* A system wide power-off event has been initiated. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_SOLO_BTN_FLY_CLICK=42001, /* FLY button has been clicked. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_SOLO_BTN_FLY_HOLD=42002, /* FLY button has been held for 1.5 seconds. |Takeoff altitude| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_SOLO_BTN_PAUSE_CLICK=42003, /* PAUSE button has been clicked. |1 if Solo is in a shot mode, 0 otherwise| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_START_MAG_CAL=42424, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Automatically retry on failure (0=no retry, 1=retry).| Save without user input (0=require input, 1=autosave).| Delay (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_ACCEPT_MAG_CAL=42425, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CANCEL_MAG_CAL=42426, /* Cancel a running magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_SET_FACTORY_TEST_MODE=42427, /* Command autopilot to get into factory test/diagnostic mode |0 means get out of test mode, 1 means get into test mode| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SEND_BANNER=42428, /* Reply with the version banner |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_GIMBAL_RESET=42501, /* Causes the gimbal to reset and boot as if it was just powered on |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_GIMBAL_AXIS_CALIBRATION_STATUS=42502, /* Reports progress and success or failure of gimbal axis calibration procedure |Gimbal axis we're reporting calibration progress for| Current calibration progress for this axis, 0x64=100%| Status of the calibration| Empty| Empty| Empty| Empty| */ + MAV_CMD_GIMBAL_REQUEST_AXIS_CALIBRATION=42503, /* Starts commutation calibration on the gimbal |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_GIMBAL_FULL_RESET=42505, /* Erases gimbal application and parameters |Magic number| Magic number| Magic number| Magic number| Magic number| Magic number| Magic number| */ + MAV_CMD_ENUM_END=42506, /* | */ +} MAV_CMD; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_LIMITS_STATE +#define HAVE_ENUM_LIMITS_STATE +typedef enum LIMITS_STATE +{ + LIMITS_INIT=0, /* pre-initialization | */ + LIMITS_DISABLED=1, /* disabled | */ + LIMITS_ENABLED=2, /* checking limits | */ + LIMITS_TRIGGERED=3, /* a limit has been breached | */ + LIMITS_RECOVERING=4, /* taking action eg. RTL | */ + LIMITS_RECOVERED=5, /* we're no longer in breach of a limit | */ + LIMITS_STATE_ENUM_END=6, /* | */ +} LIMITS_STATE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_LIMIT_MODULE +#define HAVE_ENUM_LIMIT_MODULE +typedef enum LIMIT_MODULE +{ + LIMIT_GPSLOCK=1, /* pre-initialization | */ + LIMIT_GEOFENCE=2, /* disabled | */ + LIMIT_ALTITUDE=4, /* checking limits | */ + LIMIT_MODULE_ENUM_END=5, /* | */ +} LIMIT_MODULE; +#endif + +/** @brief Flags in RALLY_POINT message */ +#ifndef HAVE_ENUM_RALLY_FLAGS +#define HAVE_ENUM_RALLY_FLAGS +typedef enum RALLY_FLAGS +{ + FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing. | */ + LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. | */ + RALLY_FLAGS_ENUM_END=3, /* | */ +} RALLY_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_PARACHUTE_ACTION +#define HAVE_ENUM_PARACHUTE_ACTION +typedef enum PARACHUTE_ACTION +{ + PARACHUTE_DISABLE=0, /* Disable parachute release | */ + PARACHUTE_ENABLE=1, /* Enable parachute release | */ + PARACHUTE_RELEASE=2, /* Release parachute | */ + PARACHUTE_ACTION_ENUM_END=3, /* | */ +} PARACHUTE_ACTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +#define HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +typedef enum MOTOR_TEST_THROTTLE_TYPE +{ + MOTOR_TEST_THROTTLE_PERCENT=0, /* throttle as a percentage from 0 ~ 100 | */ + MOTOR_TEST_THROTTLE_PWM=1, /* throttle as an absolute PWM value (normally in range of 1000~2000) | */ + MOTOR_TEST_THROTTLE_PILOT=2, /* throttle pass-through from pilot's transmitter | */ + MOTOR_TEST_THROTTLE_TYPE_ENUM_END=3, /* | */ +} MOTOR_TEST_THROTTLE_TYPE; +#endif + +/** @brief Gripper actions. */ +#ifndef HAVE_ENUM_GRIPPER_ACTIONS +#define HAVE_ENUM_GRIPPER_ACTIONS +typedef enum GRIPPER_ACTIONS +{ + GRIPPER_ACTION_RELEASE=0, /* gripper release of cargo | */ + GRIPPER_ACTION_GRAB=1, /* gripper grabs onto cargo | */ + GRIPPER_ACTIONS_ENUM_END=2, /* | */ +} GRIPPER_ACTIONS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_CAMERA_STATUS_TYPES +#define HAVE_ENUM_CAMERA_STATUS_TYPES +typedef enum CAMERA_STATUS_TYPES +{ + CAMERA_STATUS_TYPE_HEARTBEAT=0, /* Camera heartbeat, announce camera component ID at 1hz | */ + CAMERA_STATUS_TYPE_TRIGGER=1, /* Camera image triggered | */ + CAMERA_STATUS_TYPE_DISCONNECT=2, /* Camera connection lost | */ + CAMERA_STATUS_TYPE_ERROR=3, /* Camera unknown error | */ + CAMERA_STATUS_TYPE_LOWBATT=4, /* Camera battery low. Parameter p1 shows reported voltage | */ + CAMERA_STATUS_TYPE_LOWSTORE=5, /* Camera storage low. Parameter p1 shows reported shots remaining | */ + CAMERA_STATUS_TYPE_LOWSTOREV=6, /* Camera storage low. Parameter p1 shows reported video minutes remaining | */ + CAMERA_STATUS_TYPES_ENUM_END=7, /* | */ +} CAMERA_STATUS_TYPES; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_CAMERA_FEEDBACK_FLAGS +#define HAVE_ENUM_CAMERA_FEEDBACK_FLAGS +typedef enum CAMERA_FEEDBACK_FLAGS +{ + VIDEO=1, /* Shooting video, not stills | */ + BADEXPOSURE=2, /* Unable to achieve requested exposure (e.g. shutter speed too low) | */ + CLOSEDLOOP=3, /* Closed loop feedback from camera, we know for sure it has successfully taken a picture | */ + OPENLOOP=4, /* Open loop camera, an image trigger has been requested but we can't know for sure it has successfully taken a picture | */ + CAMERA_FEEDBACK_FLAGS_ENUM_END=5, /* | */ +} CAMERA_FEEDBACK_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_MODE_GIMBAL +#define HAVE_ENUM_MAV_MODE_GIMBAL +typedef enum MAV_MODE_GIMBAL +{ + MAV_MODE_GIMBAL_UNINITIALIZED=0, /* Gimbal is powered on but has not started initializing yet | */ + MAV_MODE_GIMBAL_CALIBRATING_PITCH=1, /* Gimbal is currently running calibration on the pitch axis | */ + MAV_MODE_GIMBAL_CALIBRATING_ROLL=2, /* Gimbal is currently running calibration on the roll axis | */ + MAV_MODE_GIMBAL_CALIBRATING_YAW=3, /* Gimbal is currently running calibration on the yaw axis | */ + MAV_MODE_GIMBAL_INITIALIZED=4, /* Gimbal has finished calibrating and initializing, but is relaxed pending reception of first rate command from copter | */ + MAV_MODE_GIMBAL_ACTIVE=5, /* Gimbal is actively stabilizing | */ + MAV_MODE_GIMBAL_RATE_CMD_TIMEOUT=6, /* Gimbal is relaxed because it missed more than 10 expected rate command messages in a row. Gimbal will move back to active mode when it receives a new rate command | */ + MAV_MODE_GIMBAL_ENUM_END=7, /* | */ +} MAV_MODE_GIMBAL; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GIMBAL_AXIS +#define HAVE_ENUM_GIMBAL_AXIS +typedef enum GIMBAL_AXIS +{ + GIMBAL_AXIS_YAW=0, /* Gimbal yaw axis | */ + GIMBAL_AXIS_PITCH=1, /* Gimbal pitch axis | */ + GIMBAL_AXIS_ROLL=2, /* Gimbal roll axis | */ + GIMBAL_AXIS_ENUM_END=3, /* | */ +} GIMBAL_AXIS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GIMBAL_AXIS_CALIBRATION_STATUS +#define HAVE_ENUM_GIMBAL_AXIS_CALIBRATION_STATUS +typedef enum GIMBAL_AXIS_CALIBRATION_STATUS +{ + GIMBAL_AXIS_CALIBRATION_STATUS_IN_PROGRESS=0, /* Axis calibration is in progress | */ + GIMBAL_AXIS_CALIBRATION_STATUS_SUCCEEDED=1, /* Axis calibration succeeded | */ + GIMBAL_AXIS_CALIBRATION_STATUS_FAILED=2, /* Axis calibration failed | */ + GIMBAL_AXIS_CALIBRATION_STATUS_ENUM_END=3, /* | */ +} GIMBAL_AXIS_CALIBRATION_STATUS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GIMBAL_AXIS_CALIBRATION_REQUIRED +#define HAVE_ENUM_GIMBAL_AXIS_CALIBRATION_REQUIRED +typedef enum GIMBAL_AXIS_CALIBRATION_REQUIRED +{ + GIMBAL_AXIS_CALIBRATION_REQUIRED_UNKNOWN=0, /* Whether or not this axis requires calibration is unknown at this time | */ + GIMBAL_AXIS_CALIBRATION_REQUIRED_TRUE=1, /* This axis requires calibration | */ + GIMBAL_AXIS_CALIBRATION_REQUIRED_FALSE=2, /* This axis does not require calibration | */ + GIMBAL_AXIS_CALIBRATION_REQUIRED_ENUM_END=3, /* | */ +} GIMBAL_AXIS_CALIBRATION_REQUIRED; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_HEARTBEAT_STATUS +#define HAVE_ENUM_GOPRO_HEARTBEAT_STATUS +typedef enum GOPRO_HEARTBEAT_STATUS +{ + GOPRO_HEARTBEAT_STATUS_DISCONNECTED=0, /* No GoPro connected | */ + GOPRO_HEARTBEAT_STATUS_INCOMPATIBLE=1, /* The detected GoPro is not HeroBus compatible | */ + GOPRO_HEARTBEAT_STATUS_CONNECTED=2, /* A HeroBus compatible GoPro is connected | */ + GOPRO_HEARTBEAT_STATUS_ERROR=3, /* An unrecoverable error was encountered with the connected GoPro, it may require a power cycle | */ + GOPRO_HEARTBEAT_STATUS_ENUM_END=4, /* | */ +} GOPRO_HEARTBEAT_STATUS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_HEARTBEAT_FLAGS +#define HAVE_ENUM_GOPRO_HEARTBEAT_FLAGS +typedef enum GOPRO_HEARTBEAT_FLAGS +{ + GOPRO_FLAG_RECORDING=1, /* GoPro is currently recording | */ + GOPRO_HEARTBEAT_FLAGS_ENUM_END=2, /* | */ +} GOPRO_HEARTBEAT_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_REQUEST_STATUS +#define HAVE_ENUM_GOPRO_REQUEST_STATUS +typedef enum GOPRO_REQUEST_STATUS +{ + GOPRO_REQUEST_SUCCESS=0, /* The write message with ID indicated succeeded | */ + GOPRO_REQUEST_FAILED=1, /* The write message with ID indicated failed | */ + GOPRO_REQUEST_STATUS_ENUM_END=2, /* | */ +} GOPRO_REQUEST_STATUS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_COMMAND +#define HAVE_ENUM_GOPRO_COMMAND +typedef enum GOPRO_COMMAND +{ + GOPRO_COMMAND_POWER=0, /* (Get/Set) | */ + GOPRO_COMMAND_CAPTURE_MODE=1, /* (Get/Set) | */ + GOPRO_COMMAND_SHUTTER=2, /* (___/Set) | */ + GOPRO_COMMAND_BATTERY=3, /* (Get/___) | */ + GOPRO_COMMAND_MODEL=4, /* (Get/___) | */ + GOPRO_COMMAND_VIDEO_SETTINGS=5, /* (Get/Set) | */ + GOPRO_COMMAND_LOW_LIGHT=6, /* (Get/Set) | */ + GOPRO_COMMAND_PHOTO_RESOLUTION=7, /* (Get/Set) | */ + GOPRO_COMMAND_PHOTO_BURST_RATE=8, /* (Get/Set) | */ + GOPRO_COMMAND_PROTUNE=9, /* (Get/Set) | */ + GOPRO_COMMAND_PROTUNE_WHITE_BALANCE=10, /* (Get/Set) Hero 3+ Only | */ + GOPRO_COMMAND_PROTUNE_COLOUR=11, /* (Get/Set) Hero 3+ Only | */ + GOPRO_COMMAND_PROTUNE_GAIN=12, /* (Get/Set) Hero 3+ Only | */ + GOPRO_COMMAND_PROTUNE_SHARPNESS=13, /* (Get/Set) Hero 3+ Only | */ + GOPRO_COMMAND_PROTUNE_EXPOSURE=14, /* (Get/Set) Hero 3+ Only | */ + GOPRO_COMMAND_TIME=15, /* (Get/Set) | */ + GOPRO_COMMAND_CHARGING=16, /* (Get/Set) | */ + GOPRO_COMMAND_ENUM_END=17, /* | */ +} GOPRO_COMMAND; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_CAPTURE_MODE +#define HAVE_ENUM_GOPRO_CAPTURE_MODE +typedef enum GOPRO_CAPTURE_MODE +{ + GOPRO_CAPTURE_MODE_VIDEO=0, /* Video mode | */ + GOPRO_CAPTURE_MODE_PHOTO=1, /* Photo mode | */ + GOPRO_CAPTURE_MODE_BURST=2, /* Burst mode, hero 3+ only | */ + GOPRO_CAPTURE_MODE_TIME_LAPSE=3, /* Time lapse mode, hero 3+ only | */ + GOPRO_CAPTURE_MODE_MULTI_SHOT=4, /* Multi shot mode, hero 4 only | */ + GOPRO_CAPTURE_MODE_PLAYBACK=5, /* Playback mode, hero 4 only, silver only except when LCD or HDMI is connected to black | */ + GOPRO_CAPTURE_MODE_SETUP=6, /* Playback mode, hero 4 only | */ + GOPRO_CAPTURE_MODE_UNKNOWN=255, /* Mode not yet known | */ + GOPRO_CAPTURE_MODE_ENUM_END=256, /* | */ +} GOPRO_CAPTURE_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_RESOLUTION +#define HAVE_ENUM_GOPRO_RESOLUTION +typedef enum GOPRO_RESOLUTION +{ + GOPRO_RESOLUTION_480p=0, /* 848 x 480 (480p) | */ + GOPRO_RESOLUTION_720p=1, /* 1280 x 720 (720p) | */ + GOPRO_RESOLUTION_960p=2, /* 1280 x 960 (960p) | */ + GOPRO_RESOLUTION_1080p=3, /* 1920 x 1080 (1080p) | */ + GOPRO_RESOLUTION_1440p=4, /* 1920 x 1440 (1440p) | */ + GOPRO_RESOLUTION_2_7k_17_9=5, /* 2704 x 1440 (2.7k-17:9) | */ + GOPRO_RESOLUTION_2_7k_16_9=6, /* 2704 x 1524 (2.7k-16:9) | */ + GOPRO_RESOLUTION_2_7k_4_3=7, /* 2704 x 2028 (2.7k-4:3) | */ + GOPRO_RESOLUTION_4k_16_9=8, /* 3840 x 2160 (4k-16:9) | */ + GOPRO_RESOLUTION_4k_17_9=9, /* 4096 x 2160 (4k-17:9) | */ + GOPRO_RESOLUTION_720p_SUPERVIEW=10, /* 1280 x 720 (720p-SuperView) | */ + GOPRO_RESOLUTION_1080p_SUPERVIEW=11, /* 1920 x 1080 (1080p-SuperView) | */ + GOPRO_RESOLUTION_2_7k_SUPERVIEW=12, /* 2704 x 1520 (2.7k-SuperView) | */ + GOPRO_RESOLUTION_4k_SUPERVIEW=13, /* 3840 x 2160 (4k-SuperView) | */ + GOPRO_RESOLUTION_ENUM_END=14, /* | */ +} GOPRO_RESOLUTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_FRAME_RATE +#define HAVE_ENUM_GOPRO_FRAME_RATE +typedef enum GOPRO_FRAME_RATE +{ + GOPRO_FRAME_RATE_12=0, /* 12 FPS | */ + GOPRO_FRAME_RATE_15=1, /* 15 FPS | */ + GOPRO_FRAME_RATE_24=2, /* 24 FPS | */ + GOPRO_FRAME_RATE_25=3, /* 25 FPS | */ + GOPRO_FRAME_RATE_30=4, /* 30 FPS | */ + GOPRO_FRAME_RATE_48=5, /* 48 FPS | */ + GOPRO_FRAME_RATE_50=6, /* 50 FPS | */ + GOPRO_FRAME_RATE_60=7, /* 60 FPS | */ + GOPRO_FRAME_RATE_80=8, /* 80 FPS | */ + GOPRO_FRAME_RATE_90=9, /* 90 FPS | */ + GOPRO_FRAME_RATE_100=10, /* 100 FPS | */ + GOPRO_FRAME_RATE_120=11, /* 120 FPS | */ + GOPRO_FRAME_RATE_240=12, /* 240 FPS | */ + GOPRO_FRAME_RATE_12_5=13, /* 12.5 FPS | */ + GOPRO_FRAME_RATE_ENUM_END=14, /* | */ +} GOPRO_FRAME_RATE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_FIELD_OF_VIEW +#define HAVE_ENUM_GOPRO_FIELD_OF_VIEW +typedef enum GOPRO_FIELD_OF_VIEW +{ + GOPRO_FIELD_OF_VIEW_WIDE=0, /* 0x00: Wide | */ + GOPRO_FIELD_OF_VIEW_MEDIUM=1, /* 0x01: Medium | */ + GOPRO_FIELD_OF_VIEW_NARROW=2, /* 0x02: Narrow | */ + GOPRO_FIELD_OF_VIEW_ENUM_END=3, /* | */ +} GOPRO_FIELD_OF_VIEW; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_VIDEO_SETTINGS_FLAGS +#define HAVE_ENUM_GOPRO_VIDEO_SETTINGS_FLAGS +typedef enum GOPRO_VIDEO_SETTINGS_FLAGS +{ + GOPRO_VIDEO_SETTINGS_TV_MODE=1, /* 0=NTSC, 1=PAL | */ + GOPRO_VIDEO_SETTINGS_FLAGS_ENUM_END=2, /* | */ +} GOPRO_VIDEO_SETTINGS_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PHOTO_RESOLUTION +#define HAVE_ENUM_GOPRO_PHOTO_RESOLUTION +typedef enum GOPRO_PHOTO_RESOLUTION +{ + GOPRO_PHOTO_RESOLUTION_5MP_MEDIUM=0, /* 5MP Medium | */ + GOPRO_PHOTO_RESOLUTION_7MP_MEDIUM=1, /* 7MP Medium | */ + GOPRO_PHOTO_RESOLUTION_7MP_WIDE=2, /* 7MP Wide | */ + GOPRO_PHOTO_RESOLUTION_10MP_WIDE=3, /* 10MP Wide | */ + GOPRO_PHOTO_RESOLUTION_12MP_WIDE=4, /* 12MP Wide | */ + GOPRO_PHOTO_RESOLUTION_ENUM_END=5, /* | */ +} GOPRO_PHOTO_RESOLUTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PROTUNE_WHITE_BALANCE +#define HAVE_ENUM_GOPRO_PROTUNE_WHITE_BALANCE +typedef enum GOPRO_PROTUNE_WHITE_BALANCE +{ + GOPRO_PROTUNE_WHITE_BALANCE_AUTO=0, /* Auto | */ + GOPRO_PROTUNE_WHITE_BALANCE_3000K=1, /* 3000K | */ + GOPRO_PROTUNE_WHITE_BALANCE_5500K=2, /* 5500K | */ + GOPRO_PROTUNE_WHITE_BALANCE_6500K=3, /* 6500K | */ + GOPRO_PROTUNE_WHITE_BALANCE_RAW=4, /* Camera Raw | */ + GOPRO_PROTUNE_WHITE_BALANCE_ENUM_END=5, /* | */ +} GOPRO_PROTUNE_WHITE_BALANCE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PROTUNE_COLOUR +#define HAVE_ENUM_GOPRO_PROTUNE_COLOUR +typedef enum GOPRO_PROTUNE_COLOUR +{ + GOPRO_PROTUNE_COLOUR_STANDARD=0, /* Auto | */ + GOPRO_PROTUNE_COLOUR_NEUTRAL=1, /* Neutral | */ + GOPRO_PROTUNE_COLOUR_ENUM_END=2, /* | */ +} GOPRO_PROTUNE_COLOUR; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PROTUNE_GAIN +#define HAVE_ENUM_GOPRO_PROTUNE_GAIN +typedef enum GOPRO_PROTUNE_GAIN +{ + GOPRO_PROTUNE_GAIN_400=0, /* ISO 400 | */ + GOPRO_PROTUNE_GAIN_800=1, /* ISO 800 (Only Hero 4) | */ + GOPRO_PROTUNE_GAIN_1600=2, /* ISO 1600 | */ + GOPRO_PROTUNE_GAIN_3200=3, /* ISO 3200 (Only Hero 4) | */ + GOPRO_PROTUNE_GAIN_6400=4, /* ISO 6400 | */ + GOPRO_PROTUNE_GAIN_ENUM_END=5, /* | */ +} GOPRO_PROTUNE_GAIN; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PROTUNE_SHARPNESS +#define HAVE_ENUM_GOPRO_PROTUNE_SHARPNESS +typedef enum GOPRO_PROTUNE_SHARPNESS +{ + GOPRO_PROTUNE_SHARPNESS_LOW=0, /* Low Sharpness | */ + GOPRO_PROTUNE_SHARPNESS_MEDIUM=1, /* Medium Sharpness | */ + GOPRO_PROTUNE_SHARPNESS_HIGH=2, /* High Sharpness | */ + GOPRO_PROTUNE_SHARPNESS_ENUM_END=3, /* | */ +} GOPRO_PROTUNE_SHARPNESS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_PROTUNE_EXPOSURE +#define HAVE_ENUM_GOPRO_PROTUNE_EXPOSURE +typedef enum GOPRO_PROTUNE_EXPOSURE +{ + GOPRO_PROTUNE_EXPOSURE_NEG_5_0=0, /* -5.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_4_5=1, /* -4.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_4_0=2, /* -4.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_3_5=3, /* -3.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_3_0=4, /* -3.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_2_5=5, /* -2.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_NEG_2_0=6, /* -2.0 EV | */ + GOPRO_PROTUNE_EXPOSURE_NEG_1_5=7, /* -1.5 EV | */ + GOPRO_PROTUNE_EXPOSURE_NEG_1_0=8, /* -1.0 EV | */ + GOPRO_PROTUNE_EXPOSURE_NEG_0_5=9, /* -0.5 EV | */ + GOPRO_PROTUNE_EXPOSURE_ZERO=10, /* 0.0 EV | */ + GOPRO_PROTUNE_EXPOSURE_POS_0_5=11, /* +0.5 EV | */ + GOPRO_PROTUNE_EXPOSURE_POS_1_0=12, /* +1.0 EV | */ + GOPRO_PROTUNE_EXPOSURE_POS_1_5=13, /* +1.5 EV | */ + GOPRO_PROTUNE_EXPOSURE_POS_2_0=14, /* +2.0 EV | */ + GOPRO_PROTUNE_EXPOSURE_POS_2_5=15, /* +2.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_POS_3_0=16, /* +3.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_POS_3_5=17, /* +3.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_POS_4_0=18, /* +4.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_POS_4_5=19, /* +4.5 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_POS_5_0=20, /* +5.0 EV (Hero 3+ Only) | */ + GOPRO_PROTUNE_EXPOSURE_ENUM_END=21, /* | */ +} GOPRO_PROTUNE_EXPOSURE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_CHARGING +#define HAVE_ENUM_GOPRO_CHARGING +typedef enum GOPRO_CHARGING +{ + GOPRO_CHARGING_DISABLED=0, /* Charging disabled | */ + GOPRO_CHARGING_ENABLED=1, /* Charging enabled | */ + GOPRO_CHARGING_ENUM_END=2, /* | */ +} GOPRO_CHARGING; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_MODEL +#define HAVE_ENUM_GOPRO_MODEL +typedef enum GOPRO_MODEL +{ + GOPRO_MODEL_UNKNOWN=0, /* Unknown gopro model | */ + GOPRO_MODEL_HERO_3_PLUS_SILVER=1, /* Hero 3+ Silver (HeroBus not supported by GoPro) | */ + GOPRO_MODEL_HERO_3_PLUS_BLACK=2, /* Hero 3+ Black | */ + GOPRO_MODEL_HERO_4_SILVER=3, /* Hero 4 Silver | */ + GOPRO_MODEL_HERO_4_BLACK=4, /* Hero 4 Black | */ + GOPRO_MODEL_ENUM_END=5, /* | */ +} GOPRO_MODEL; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_GOPRO_BURST_RATE +#define HAVE_ENUM_GOPRO_BURST_RATE +typedef enum GOPRO_BURST_RATE +{ + GOPRO_BURST_RATE_3_IN_1_SECOND=0, /* 3 Shots / 1 Second | */ + GOPRO_BURST_RATE_5_IN_1_SECOND=1, /* 5 Shots / 1 Second | */ + GOPRO_BURST_RATE_10_IN_1_SECOND=2, /* 10 Shots / 1 Second | */ + GOPRO_BURST_RATE_10_IN_2_SECOND=3, /* 10 Shots / 2 Second | */ + GOPRO_BURST_RATE_10_IN_3_SECOND=4, /* 10 Shots / 3 Second (Hero 4 Only) | */ + GOPRO_BURST_RATE_30_IN_1_SECOND=5, /* 30 Shots / 1 Second | */ + GOPRO_BURST_RATE_30_IN_2_SECOND=6, /* 30 Shots / 2 Second | */ + GOPRO_BURST_RATE_30_IN_3_SECOND=7, /* 30 Shots / 3 Second | */ + GOPRO_BURST_RATE_30_IN_6_SECOND=8, /* 30 Shots / 6 Second | */ + GOPRO_BURST_RATE_ENUM_END=9, /* | */ +} GOPRO_BURST_RATE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_LED_CONTROL_PATTERN +#define HAVE_ENUM_LED_CONTROL_PATTERN +typedef enum LED_CONTROL_PATTERN +{ + LED_CONTROL_PATTERN_OFF=0, /* LED patterns off (return control to regular vehicle control) | */ + LED_CONTROL_PATTERN_FIRMWAREUPDATE=1, /* LEDs show pattern during firmware update | */ + LED_CONTROL_PATTERN_CUSTOM=255, /* Custom Pattern using custom bytes fields | */ + LED_CONTROL_PATTERN_ENUM_END=256, /* | */ +} LED_CONTROL_PATTERN; +#endif + +/** @brief Flags in EKF_STATUS message */ +#ifndef HAVE_ENUM_EKF_STATUS_FLAGS +#define HAVE_ENUM_EKF_STATUS_FLAGS +typedef enum EKF_STATUS_FLAGS +{ + EKF_ATTITUDE=1, /* set if EKF's attitude estimate is good | */ + EKF_VELOCITY_HORIZ=2, /* set if EKF's horizontal velocity estimate is good | */ + EKF_VELOCITY_VERT=4, /* set if EKF's vertical velocity estimate is good | */ + EKF_POS_HORIZ_REL=8, /* set if EKF's horizontal position (relative) estimate is good | */ + EKF_POS_HORIZ_ABS=16, /* set if EKF's horizontal position (absolute) estimate is good | */ + EKF_POS_VERT_ABS=32, /* set if EKF's vertical position (absolute) estimate is good | */ + EKF_POS_VERT_AGL=64, /* set if EKF's vertical position (above ground) estimate is good | */ + EKF_CONST_POS_MODE=128, /* EKF is in constant position mode and does not know it's absolute or relative position | */ + EKF_PRED_POS_HORIZ_REL=256, /* set if EKF's predicted horizontal position (relative) estimate is good | */ + EKF_PRED_POS_HORIZ_ABS=512, /* set if EKF's predicted horizontal position (absolute) estimate is good | */ + EKF_STATUS_FLAGS_ENUM_END=513, /* | */ +} EKF_STATUS_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAG_CAL_STATUS +#define HAVE_ENUM_MAG_CAL_STATUS +typedef enum MAG_CAL_STATUS +{ + MAG_CAL_NOT_STARTED=0, /* | */ + MAG_CAL_WAITING_TO_START=1, /* | */ + MAG_CAL_RUNNING_STEP_ONE=2, /* | */ + MAG_CAL_RUNNING_STEP_TWO=3, /* | */ + MAG_CAL_SUCCESS=4, /* | */ + MAG_CAL_FAILED=5, /* | */ + MAG_CAL_STATUS_ENUM_END=6, /* | */ +} MAG_CAL_STATUS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_PID_TUNING_AXIS +#define HAVE_ENUM_PID_TUNING_AXIS +typedef enum PID_TUNING_AXIS +{ + PID_TUNING_ROLL=1, /* | */ + PID_TUNING_PITCH=2, /* | */ + PID_TUNING_YAW=3, /* | */ + PID_TUNING_AXIS_ENUM_END=4, /* | */ +} PID_TUNING_AXIS; +#endif + +/** @brief Special ACK block numbers control activation of dataflash log streaming */ +#ifndef HAVE_ENUM_MAV_REMOTE_LOG_DATA_BLOCK_COMMANDS +#define HAVE_ENUM_MAV_REMOTE_LOG_DATA_BLOCK_COMMANDS +typedef enum MAV_REMOTE_LOG_DATA_BLOCK_COMMANDS +{ + MAV_REMOTE_LOG_DATA_BLOCK_STOP=2147483645, /* UAV to stop sending DataFlash blocks | */ + MAV_REMOTE_LOG_DATA_BLOCK_START=2147483646, /* UAV to start sending DataFlash blocks | */ + MAV_REMOTE_LOG_DATA_BLOCK_COMMANDS_ENUM_END=2147483647, /* | */ +} MAV_REMOTE_LOG_DATA_BLOCK_COMMANDS; +#endif + +/** @brief Possible remote log data block statuses */ +#ifndef HAVE_ENUM_MAV_REMOTE_LOG_DATA_BLOCK_STATUSES +#define HAVE_ENUM_MAV_REMOTE_LOG_DATA_BLOCK_STATUSES +typedef enum MAV_REMOTE_LOG_DATA_BLOCK_STATUSES +{ + MAV_REMOTE_LOG_DATA_BLOCK_NACK=0, /* This block has NOT been received | */ + MAV_REMOTE_LOG_DATA_BLOCK_ACK=1, /* This block has been received | */ + MAV_REMOTE_LOG_DATA_BLOCK_STATUSES_ENUM_END=2, /* | */ +} MAV_REMOTE_LOG_DATA_BLOCK_STATUSES; +#endif + +#include "../common/common.h" + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_sensor_offsets.h" +#include "./mavlink_msg_set_mag_offsets.h" +#include "./mavlink_msg_meminfo.h" +#include "./mavlink_msg_ap_adc.h" +#include "./mavlink_msg_digicam_configure.h" +#include "./mavlink_msg_digicam_control.h" +#include "./mavlink_msg_mount_configure.h" +#include "./mavlink_msg_mount_control.h" +#include "./mavlink_msg_mount_status.h" +#include "./mavlink_msg_fence_point.h" +#include "./mavlink_msg_fence_fetch_point.h" +#include "./mavlink_msg_fence_status.h" +#include "./mavlink_msg_ahrs.h" +#include "./mavlink_msg_simstate.h" +#include "./mavlink_msg_hwstatus.h" +#include "./mavlink_msg_radio.h" +#include "./mavlink_msg_limits_status.h" +#include "./mavlink_msg_wind.h" +#include "./mavlink_msg_data16.h" +#include "./mavlink_msg_data32.h" +#include "./mavlink_msg_data64.h" +#include "./mavlink_msg_data96.h" +#include "./mavlink_msg_rangefinder.h" +#include "./mavlink_msg_airspeed_autocal.h" +#include "./mavlink_msg_rally_point.h" +#include "./mavlink_msg_rally_fetch_point.h" +#include "./mavlink_msg_compassmot_status.h" +#include "./mavlink_msg_ahrs2.h" +#include "./mavlink_msg_camera_status.h" +#include "./mavlink_msg_camera_feedback.h" +#include "./mavlink_msg_battery2.h" +#include "./mavlink_msg_ahrs3.h" +#include "./mavlink_msg_autopilot_version_request.h" +#include "./mavlink_msg_remote_log_data_block.h" +#include "./mavlink_msg_remote_log_block_status.h" +#include "./mavlink_msg_led_control.h" +#include "./mavlink_msg_mag_cal_progress.h" +#include "./mavlink_msg_mag_cal_report.h" +#include "./mavlink_msg_ekf_status_report.h" +#include "./mavlink_msg_pid_tuning.h" +#include "./mavlink_msg_gimbal_report.h" +#include "./mavlink_msg_gimbal_control.h" +#include "./mavlink_msg_gimbal_torque_cmd_report.h" +#include "./mavlink_msg_gopro_heartbeat.h" +#include "./mavlink_msg_gopro_get_request.h" +#include "./mavlink_msg_gopro_get_response.h" +#include "./mavlink_msg_gopro_set_request.h" +#include "./mavlink_msg_gopro_set_response.h" +#include "./mavlink_msg_gps_accuracy.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_ARDUPILOTMEGA_H diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink.h new file mode 100644 index 0000000..edf27d6 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink.h @@ -0,0 +1,27 @@ +/** @file + * @brief MAVLink comm protocol built from ardupilotmega.xml + * @see http://mavlink.org + */ +#ifndef MAVLINK_H +#define MAVLINK_H + +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#include "version.h" +#include "ardupilotmega.h" + +#endif // MAVLINK_H diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs.h new file mode 100644 index 0000000..50ddc79 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs.h @@ -0,0 +1,353 @@ +// MESSAGE AHRS PACKING + +#define MAVLINK_MSG_ID_AHRS 163 + +typedef struct __mavlink_ahrs_t +{ + float omegaIx; ///< X gyro drift estimate rad/s + float omegaIy; ///< Y gyro drift estimate rad/s + float omegaIz; ///< Z gyro drift estimate rad/s + float accel_weight; ///< average accel_weight + float renorm_val; ///< average renormalisation value + float error_rp; ///< average error_roll_pitch value + float error_yaw; ///< average error_yaw value +} mavlink_ahrs_t; + +#define MAVLINK_MSG_ID_AHRS_LEN 28 +#define MAVLINK_MSG_ID_163_LEN 28 + +#define MAVLINK_MSG_ID_AHRS_CRC 127 +#define MAVLINK_MSG_ID_163_CRC 127 + + + +#define MAVLINK_MESSAGE_INFO_AHRS { \ + "AHRS", \ + 7, \ + { { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \ + { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \ + { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \ + { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \ + { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \ + { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \ + { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \ + } \ +} + + +/** + * @brief Pack a ahrs message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS_LEN]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN); +#endif +} + +/** + * @brief Pack a ahrs message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS_LEN]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN); +#endif +} + +/** + * @brief Encode a ahrs struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** + * @brief Encode a ahrs struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** + * @brief Send a ahrs message + * @param chan MAVLink channel to send the message + * + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS_LEN]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#else + mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf; + packet->omegaIx = omegaIx; + packet->omegaIy = omegaIy; + packet->omegaIz = omegaIz; + packet->accel_weight = accel_weight; + packet->renorm_val = renorm_val; + packet->error_rp = error_rp; + packet->error_yaw = error_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AHRS UNPACKING + + +/** + * @brief Get field omegaIx from ahrs message + * + * @return X gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field omegaIy from ahrs message + * + * @return Y gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field omegaIz from ahrs message + * + * @return Z gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field accel_weight from ahrs message + * + * @return average accel_weight + */ +static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field renorm_val from ahrs message + * + * @return average renormalisation value + */ +static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field error_rp from ahrs message + * + * @return average error_roll_pitch value + */ +static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field error_yaw from ahrs message + * + * @return average error_yaw value + */ +static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a ahrs message into a struct + * + * @param msg The message to decode + * @param ahrs C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg); + ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg); + ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg); + ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg); + ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg); + ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg); + ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg); +#else + memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs2.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs2.h new file mode 100644 index 0000000..33e0066 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs2.h @@ -0,0 +1,329 @@ +// MESSAGE AHRS2 PACKING + +#define MAVLINK_MSG_ID_AHRS2 178 + +typedef struct __mavlink_ahrs2_t +{ + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float altitude; ///< Altitude (MSL) + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 +} mavlink_ahrs2_t; + +#define MAVLINK_MSG_ID_AHRS2_LEN 24 +#define MAVLINK_MSG_ID_178_LEN 24 + +#define MAVLINK_MSG_ID_AHRS2_CRC 47 +#define MAVLINK_MSG_ID_178_CRC 47 + + + +#define MAVLINK_MESSAGE_INFO_AHRS2 { \ + "AHRS2", \ + 6, \ + { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \ + { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \ + } \ +} + + +/** + * @brief Pack a ahrs2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Pack a ahrs2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Encode a ahrs2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Encode a ahrs2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Send a ahrs2 message + * @param chan MAVLink channel to send the message + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AHRS2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t *packet = (mavlink_ahrs2_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->altitude = altitude; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AHRS2 UNPACKING + + +/** + * @brief Get field roll from ahrs2 message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pitch from ahrs2 message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from ahrs2 message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude from ahrs2 message + * + * @return Altitude (MSL) + */ +static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field lat from ahrs2 message + * + * @return Latitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lng from ahrs2 message + * + * @return Longitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Decode a ahrs2 message into a struct + * + * @param msg The message to decode + * @param ahrs2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg); + ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg); + ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg); + ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg); + ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg); + ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg); +#else + memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs3.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs3.h new file mode 100644 index 0000000..32fb8f4 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs3.h @@ -0,0 +1,425 @@ +// MESSAGE AHRS3 PACKING + +#define MAVLINK_MSG_ID_AHRS3 182 + +typedef struct __mavlink_ahrs3_t +{ + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float altitude; ///< Altitude (MSL) + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 + float v1; ///< test variable1 + float v2; ///< test variable2 + float v3; ///< test variable3 + float v4; ///< test variable4 +} mavlink_ahrs3_t; + +#define MAVLINK_MSG_ID_AHRS3_LEN 40 +#define MAVLINK_MSG_ID_182_LEN 40 + +#define MAVLINK_MSG_ID_AHRS3_CRC 229 +#define MAVLINK_MSG_ID_182_CRC 229 + + + +#define MAVLINK_MESSAGE_INFO_AHRS3 { \ + "AHRS3", \ + 10, \ + { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs3_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs3_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs3_t, yaw) }, \ + { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs3_t, altitude) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs3_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs3_t, lng) }, \ + { "v1", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs3_t, v1) }, \ + { "v2", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_ahrs3_t, v2) }, \ + { "v3", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_ahrs3_t, v3) }, \ + { "v4", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_ahrs3_t, v4) }, \ + } \ +} + + +/** + * @brief Pack a ahrs3 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @param v1 test variable1 + * @param v2 test variable2 + * @param v3 test variable3 + * @param v4 test variable4 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng, float v1, float v2, float v3, float v4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS3_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + _mav_put_float(buf, 24, v1); + _mav_put_float(buf, 28, v2); + _mav_put_float(buf, 32, v3); + _mav_put_float(buf, 36, v4); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS3_LEN); +#else + mavlink_ahrs3_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + packet.v1 = v1; + packet.v2 = v2; + packet.v3 = v3; + packet.v4 = v4; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +} + +/** + * @brief Pack a ahrs3 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @param v1 test variable1 + * @param v2 test variable2 + * @param v3 test variable3 + * @param v4 test variable4 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng,float v1,float v2,float v3,float v4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS3_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + _mav_put_float(buf, 24, v1); + _mav_put_float(buf, 28, v2); + _mav_put_float(buf, 32, v3); + _mav_put_float(buf, 36, v4); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS3_LEN); +#else + mavlink_ahrs3_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + packet.v1 = v1; + packet.v2 = v2; + packet.v3 = v3; + packet.v4 = v4; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +} + +/** + * @brief Encode a ahrs3 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs3_t* ahrs3) +{ + return mavlink_msg_ahrs3_pack(system_id, component_id, msg, ahrs3->roll, ahrs3->pitch, ahrs3->yaw, ahrs3->altitude, ahrs3->lat, ahrs3->lng, ahrs3->v1, ahrs3->v2, ahrs3->v3, ahrs3->v4); +} + +/** + * @brief Encode a ahrs3 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs3_t* ahrs3) +{ + return mavlink_msg_ahrs3_pack_chan(system_id, component_id, chan, msg, ahrs3->roll, ahrs3->pitch, ahrs3->yaw, ahrs3->altitude, ahrs3->lat, ahrs3->lng, ahrs3->v1, ahrs3->v2, ahrs3->v3, ahrs3->v4); +} + +/** + * @brief Send a ahrs3 message + * @param chan MAVLink channel to send the message + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @param v1 test variable1 + * @param v2 test variable2 + * @param v3 test variable3 + * @param v4 test variable4 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs3_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng, float v1, float v2, float v3, float v4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS3_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + _mav_put_float(buf, 24, v1); + _mav_put_float(buf, 28, v2); + _mav_put_float(buf, 32, v3); + _mav_put_float(buf, 36, v4); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, buf, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, buf, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +#else + mavlink_ahrs3_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + packet.v1 = v1; + packet.v2 = v2; + packet.v3 = v3; + packet.v4 = v4; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, (const char *)&packet, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, (const char *)&packet, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AHRS3_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng, float v1, float v2, float v3, float v4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + _mav_put_float(buf, 24, v1); + _mav_put_float(buf, 28, v2); + _mav_put_float(buf, 32, v3); + _mav_put_float(buf, 36, v4); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, buf, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, buf, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +#else + mavlink_ahrs3_t *packet = (mavlink_ahrs3_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->altitude = altitude; + packet->lat = lat; + packet->lng = lng; + packet->v1 = v1; + packet->v2 = v2; + packet->v3 = v3; + packet->v4 = v4; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, (const char *)packet, MAVLINK_MSG_ID_AHRS3_LEN, MAVLINK_MSG_ID_AHRS3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS3, (const char *)packet, MAVLINK_MSG_ID_AHRS3_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AHRS3 UNPACKING + + +/** + * @brief Get field roll from ahrs3 message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_ahrs3_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pitch from ahrs3 message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_ahrs3_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from ahrs3 message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_ahrs3_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude from ahrs3 message + * + * @return Altitude (MSL) + */ +static inline float mavlink_msg_ahrs3_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field lat from ahrs3 message + * + * @return Latitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs3_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lng from ahrs3 message + * + * @return Longitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs3_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field v1 from ahrs3 message + * + * @return test variable1 + */ +static inline float mavlink_msg_ahrs3_get_v1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field v2 from ahrs3 message + * + * @return test variable2 + */ +static inline float mavlink_msg_ahrs3_get_v2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field v3 from ahrs3 message + * + * @return test variable3 + */ +static inline float mavlink_msg_ahrs3_get_v3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field v4 from ahrs3 message + * + * @return test variable4 + */ +static inline float mavlink_msg_ahrs3_get_v4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a ahrs3 message into a struct + * + * @param msg The message to decode + * @param ahrs3 C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs3_decode(const mavlink_message_t* msg, mavlink_ahrs3_t* ahrs3) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs3->roll = mavlink_msg_ahrs3_get_roll(msg); + ahrs3->pitch = mavlink_msg_ahrs3_get_pitch(msg); + ahrs3->yaw = mavlink_msg_ahrs3_get_yaw(msg); + ahrs3->altitude = mavlink_msg_ahrs3_get_altitude(msg); + ahrs3->lat = mavlink_msg_ahrs3_get_lat(msg); + ahrs3->lng = mavlink_msg_ahrs3_get_lng(msg); + ahrs3->v1 = mavlink_msg_ahrs3_get_v1(msg); + ahrs3->v2 = mavlink_msg_ahrs3_get_v2(msg); + ahrs3->v3 = mavlink_msg_ahrs3_get_v3(msg); + ahrs3->v4 = mavlink_msg_ahrs3_get_v4(msg); +#else + memcpy(ahrs3, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS3_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_airspeed_autocal.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_airspeed_autocal.h new file mode 100644 index 0000000..521831c --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -0,0 +1,473 @@ +// MESSAGE AIRSPEED_AUTOCAL PACKING + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 + +typedef struct __mavlink_airspeed_autocal_t +{ + float vx; ///< GPS velocity north m/s + float vy; ///< GPS velocity east m/s + float vz; ///< GPS velocity down m/s + float diff_pressure; ///< Differential pressure pascals + float EAS2TAS; ///< Estimated to true airspeed ratio + float ratio; ///< Airspeed ratio + float state_x; ///< EKF state x + float state_y; ///< EKF state y + float state_z; ///< EKF state z + float Pax; ///< EKF Pax + float Pby; ///< EKF Pby + float Pcz; ///< EKF Pcz +} mavlink_airspeed_autocal_t; + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 +#define MAVLINK_MSG_ID_174_LEN 48 + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 +#define MAVLINK_MSG_ID_174_CRC 167 + + + +#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ + "AIRSPEED_AUTOCAL", \ + 12, \ + { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ + { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ + { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ + { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ + { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ + { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ + { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ + { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ + { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ + } \ +} + + +/** + * @brief Pack a airspeed_autocal message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Pack a airspeed_autocal message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Encode a airspeed_autocal struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Encode a airspeed_autocal struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Send a airspeed_autocal message + * @param chan MAVLink channel to send the message + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_airspeed_autocal_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t *packet = (mavlink_airspeed_autocal_t *)msgbuf; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->diff_pressure = diff_pressure; + packet->EAS2TAS = EAS2TAS; + packet->ratio = ratio; + packet->state_x = state_x; + packet->state_y = state_y; + packet->state_z = state_z; + packet->Pax = Pax; + packet->Pby = Pby; + packet->Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AIRSPEED_AUTOCAL UNPACKING + + +/** + * @brief Get field vx from airspeed_autocal message + * + * @return GPS velocity north m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field vy from airspeed_autocal message + * + * @return GPS velocity east m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field vz from airspeed_autocal message + * + * @return GPS velocity down m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field diff_pressure from airspeed_autocal message + * + * @return Differential pressure pascals + */ +static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field EAS2TAS from airspeed_autocal message + * + * @return Estimated to true airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field ratio from airspeed_autocal message + * + * @return Airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field state_x from airspeed_autocal message + * + * @return EKF state x + */ +static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field state_y from airspeed_autocal message + * + * @return EKF state y + */ +static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field state_z from airspeed_autocal message + * + * @return EKF state z + */ +static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field Pax from airspeed_autocal message + * + * @return EKF Pax + */ +static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field Pby from airspeed_autocal message + * + * @return EKF Pby + */ +static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field Pcz from airspeed_autocal message + * + * @return EKF Pcz + */ +static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a airspeed_autocal message into a struct + * + * @param msg The message to decode + * @param airspeed_autocal C-struct to decode the message contents into + */ +static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) +{ +#if MAVLINK_NEED_BYTE_SWAP + airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); + airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); + airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); + airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); + airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); + airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); + airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); + airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); + airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); + airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); + airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); + airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); +#else + memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ap_adc.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ap_adc.h new file mode 100644 index 0000000..3c18e64 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ap_adc.h @@ -0,0 +1,329 @@ +// MESSAGE AP_ADC PACKING + +#define MAVLINK_MSG_ID_AP_ADC 153 + +typedef struct __mavlink_ap_adc_t +{ + uint16_t adc1; ///< ADC output 1 + uint16_t adc2; ///< ADC output 2 + uint16_t adc3; ///< ADC output 3 + uint16_t adc4; ///< ADC output 4 + uint16_t adc5; ///< ADC output 5 + uint16_t adc6; ///< ADC output 6 +} mavlink_ap_adc_t; + +#define MAVLINK_MSG_ID_AP_ADC_LEN 12 +#define MAVLINK_MSG_ID_153_LEN 12 + +#define MAVLINK_MSG_ID_AP_ADC_CRC 188 +#define MAVLINK_MSG_ID_153_CRC 188 + + + +#define MAVLINK_MESSAGE_INFO_AP_ADC { \ + "AP_ADC", \ + 6, \ + { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ + { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ + { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ + { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ + { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ + { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ + } \ +} + + +/** + * @brief Pack a ap_adc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +} + +/** + * @brief Pack a ap_adc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +} + +/** + * @brief Encode a ap_adc struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** + * @brief Encode a ap_adc struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** + * @brief Send a ap_adc message + * @param chan MAVLink channel to send the message + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AP_ADC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ap_adc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#else + mavlink_ap_adc_t *packet = (mavlink_ap_adc_t *)msgbuf; + packet->adc1 = adc1; + packet->adc2 = adc2; + packet->adc3 = adc3; + packet->adc4 = adc4; + packet->adc5 = adc5; + packet->adc6 = adc6; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AP_ADC UNPACKING + + +/** + * @brief Get field adc1 from ap_adc message + * + * @return ADC output 1 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field adc2 from ap_adc message + * + * @return ADC output 2 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field adc3 from ap_adc message + * + * @return ADC output 3 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field adc4 from ap_adc message + * + * @return ADC output 4 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field adc5 from ap_adc message + * + * @return ADC output 5 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field adc6 from ap_adc message + * + * @return ADC output 6 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Decode a ap_adc message into a struct + * + * @param msg The message to decode + * @param ap_adc C-struct to decode the message contents into + */ +static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) +{ +#if MAVLINK_NEED_BYTE_SWAP + ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); + ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); + ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); + ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); + ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); + ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); +#else + memcpy(ap_adc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_autopilot_version_request.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_autopilot_version_request.h new file mode 100644 index 0000000..0420d5d --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_autopilot_version_request.h @@ -0,0 +1,233 @@ +// MESSAGE AUTOPILOT_VERSION_REQUEST PACKING + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST 183 + +typedef struct __mavlink_autopilot_version_request_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_autopilot_version_request_t; + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN 2 +#define MAVLINK_MSG_ID_183_LEN 2 + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC 85 +#define MAVLINK_MSG_ID_183_CRC 85 + + + +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION_REQUEST { \ + "AUTOPILOT_VERSION_REQUEST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_autopilot_version_request_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_autopilot_version_request_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a autopilot_version_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#else + mavlink_autopilot_version_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a autopilot_version_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#else + mavlink_autopilot_version_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a autopilot_version_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param autopilot_version_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_version_request_t* autopilot_version_request) +{ + return mavlink_msg_autopilot_version_request_pack(system_id, component_id, msg, autopilot_version_request->target_system, autopilot_version_request->target_component); +} + +/** + * @brief Encode a autopilot_version_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param autopilot_version_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_version_request_t* autopilot_version_request) +{ + return mavlink_msg_autopilot_version_request_pack_chan(system_id, component_id, chan, msg, autopilot_version_request->target_system, autopilot_version_request->target_component); +} + +/** + * @brief Send a autopilot_version_request message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_autopilot_version_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +#else + mavlink_autopilot_version_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_autopilot_version_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +#else + mavlink_autopilot_version_request_t *packet = (mavlink_autopilot_version_request_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AUTOPILOT_VERSION_REQUEST UNPACKING + + +/** + * @brief Get field target_system from autopilot_version_request message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_autopilot_version_request_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from autopilot_version_request message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_autopilot_version_request_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a autopilot_version_request message into a struct + * + * @param msg The message to decode + * @param autopilot_version_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_autopilot_version_request_decode(const mavlink_message_t* msg, mavlink_autopilot_version_request_t* autopilot_version_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + autopilot_version_request->target_system = mavlink_msg_autopilot_version_request_get_target_system(msg); + autopilot_version_request->target_component = mavlink_msg_autopilot_version_request_get_target_component(msg); +#else + memcpy(autopilot_version_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_battery2.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_battery2.h new file mode 100644 index 0000000..c8c2bbf --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_battery2.h @@ -0,0 +1,233 @@ +// MESSAGE BATTERY2 PACKING + +#define MAVLINK_MSG_ID_BATTERY2 181 + +typedef struct __mavlink_battery2_t +{ + uint16_t voltage; ///< voltage in millivolts + int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current +} mavlink_battery2_t; + +#define MAVLINK_MSG_ID_BATTERY2_LEN 4 +#define MAVLINK_MSG_ID_181_LEN 4 + +#define MAVLINK_MSG_ID_BATTERY2_CRC 174 +#define MAVLINK_MSG_ID_181_CRC 174 + + + +#define MAVLINK_MESSAGE_INFO_BATTERY2 { \ + "BATTERY2", \ + 2, \ + { { "voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery2_t, voltage) }, \ + { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_battery2_t, current_battery) }, \ + } \ +} + + +/** + * @brief Pack a battery2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param voltage voltage in millivolts + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t voltage, int16_t current_battery) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; + _mav_put_uint16_t(buf, 0, voltage); + _mav_put_int16_t(buf, 2, current_battery); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY2_LEN); +#else + mavlink_battery2_t packet; + packet.voltage = voltage; + packet.current_battery = current_battery; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +} + +/** + * @brief Pack a battery2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param voltage voltage in millivolts + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t voltage,int16_t current_battery) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; + _mav_put_uint16_t(buf, 0, voltage); + _mav_put_int16_t(buf, 2, current_battery); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY2_LEN); +#else + mavlink_battery2_t packet; + packet.voltage = voltage; + packet.current_battery = current_battery; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +} + +/** + * @brief Encode a battery2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param battery2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery2_t* battery2) +{ + return mavlink_msg_battery2_pack(system_id, component_id, msg, battery2->voltage, battery2->current_battery); +} + +/** + * @brief Encode a battery2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery2_t* battery2) +{ + return mavlink_msg_battery2_pack_chan(system_id, component_id, chan, msg, battery2->voltage, battery2->current_battery); +} + +/** + * @brief Send a battery2 message + * @param chan MAVLink channel to send the message + * + * @param voltage voltage in millivolts + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_battery2_send(mavlink_channel_t chan, uint16_t voltage, int16_t current_battery) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY2_LEN]; + _mav_put_uint16_t(buf, 0, voltage); + _mav_put_int16_t(buf, 2, current_battery); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +#else + mavlink_battery2_t packet; + packet.voltage = voltage; + packet.current_battery = current_battery; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)&packet, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)&packet, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_BATTERY2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_battery2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t voltage, int16_t current_battery) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, voltage); + _mav_put_int16_t(buf, 2, current_battery); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, buf, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +#else + mavlink_battery2_t *packet = (mavlink_battery2_t *)msgbuf; + packet->voltage = voltage; + packet->current_battery = current_battery; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)packet, MAVLINK_MSG_ID_BATTERY2_LEN, MAVLINK_MSG_ID_BATTERY2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY2, (const char *)packet, MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE BATTERY2 UNPACKING + + +/** + * @brief Get field voltage from battery2 message + * + * @return voltage in millivolts + */ +static inline uint16_t mavlink_msg_battery2_get_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field current_battery from battery2 message + * + * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + */ +static inline int16_t mavlink_msg_battery2_get_current_battery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Decode a battery2 message into a struct + * + * @param msg The message to decode + * @param battery2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_battery2_decode(const mavlink_message_t* msg, mavlink_battery2_t* battery2) +{ +#if MAVLINK_NEED_BYTE_SWAP + battery2->voltage = mavlink_msg_battery2_get_voltage(msg); + battery2->current_battery = mavlink_msg_battery2_get_current_battery(msg); +#else + memcpy(battery2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BATTERY2_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_feedback.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_feedback.h new file mode 100644 index 0000000..6a9748d --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_feedback.h @@ -0,0 +1,497 @@ +// MESSAGE CAMERA_FEEDBACK PACKING + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK 180 + +typedef struct __mavlink_camera_feedback_t +{ + uint64_t time_usec; ///< Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) + int32_t lat; ///< Latitude in (deg * 1E7) + int32_t lng; ///< Longitude in (deg * 1E7) + float alt_msl; ///< Altitude Absolute (meters AMSL) + float alt_rel; ///< Altitude Relative (meters above HOME location) + float roll; ///< Camera Roll angle (earth frame, degrees, +-180) + float pitch; ///< Camera Pitch angle (earth frame, degrees, +-180) + float yaw; ///< Camera Yaw (earth frame, degrees, 0-360, true) + float foc_len; ///< Focal Length (mm) + uint16_t img_idx; ///< Image index + uint8_t target_system; ///< System ID + uint8_t cam_idx; ///< Camera ID + uint8_t flags; ///< See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask +} mavlink_camera_feedback_t; + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN 45 +#define MAVLINK_MSG_ID_180_LEN 45 + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC 52 +#define MAVLINK_MSG_ID_180_CRC 52 + + + +#define MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK { \ + "CAMERA_FEEDBACK", \ + 13, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_feedback_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_feedback_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_feedback_t, lng) }, \ + { "alt_msl", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_feedback_t, alt_msl) }, \ + { "alt_rel", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_feedback_t, alt_rel) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_feedback_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_feedback_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_feedback_t, yaw) }, \ + { "foc_len", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_feedback_t, foc_len) }, \ + { "img_idx", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_camera_feedback_t, img_idx) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_camera_feedback_t, target_system) }, \ + { "cam_idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_camera_feedback_t, cam_idx) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_camera_feedback_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a camera_feedback message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt_msl Altitude Absolute (meters AMSL) + * @param alt_rel Altitude Relative (meters above HOME location) + * @param roll Camera Roll angle (earth frame, degrees, +-180) + * @param pitch Camera Pitch angle (earth frame, degrees, +-180) + * @param yaw Camera Yaw (earth frame, degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_feedback_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt_msl, float alt_rel, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt_msl); + _mav_put_float(buf, 20, alt_rel); + _mav_put_float(buf, 24, roll); + _mav_put_float(buf, 28, pitch); + _mav_put_float(buf, 32, yaw); + _mav_put_float(buf, 36, foc_len); + _mav_put_uint16_t(buf, 40, img_idx); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, cam_idx); + _mav_put_uint8_t(buf, 44, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt_msl = alt_msl; + packet.alt_rel = alt_rel; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FEEDBACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} + +/** + * @brief Pack a camera_feedback message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt_msl Altitude Absolute (meters AMSL) + * @param alt_rel Altitude Relative (meters above HOME location) + * @param roll Camera Roll angle (earth frame, degrees, +-180) + * @param pitch Camera Pitch angle (earth frame, degrees, +-180) + * @param yaw Camera Yaw (earth frame, degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_feedback_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t target_system,uint8_t cam_idx,uint16_t img_idx,int32_t lat,int32_t lng,float alt_msl,float alt_rel,float roll,float pitch,float yaw,float foc_len,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt_msl); + _mav_put_float(buf, 20, alt_rel); + _mav_put_float(buf, 24, roll); + _mav_put_float(buf, 28, pitch); + _mav_put_float(buf, 32, yaw); + _mav_put_float(buf, 36, foc_len); + _mav_put_uint16_t(buf, 40, img_idx); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, cam_idx); + _mav_put_uint8_t(buf, 44, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt_msl = alt_msl; + packet.alt_rel = alt_rel; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FEEDBACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} + +/** + * @brief Encode a camera_feedback struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_feedback C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_feedback_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_feedback_t* camera_feedback) +{ + return mavlink_msg_camera_feedback_pack(system_id, component_id, msg, camera_feedback->time_usec, camera_feedback->target_system, camera_feedback->cam_idx, camera_feedback->img_idx, camera_feedback->lat, camera_feedback->lng, camera_feedback->alt_msl, camera_feedback->alt_rel, camera_feedback->roll, camera_feedback->pitch, camera_feedback->yaw, camera_feedback->foc_len, camera_feedback->flags); +} + +/** + * @brief Encode a camera_feedback struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_feedback C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_feedback_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_feedback_t* camera_feedback) +{ + return mavlink_msg_camera_feedback_pack_chan(system_id, component_id, chan, msg, camera_feedback->time_usec, camera_feedback->target_system, camera_feedback->cam_idx, camera_feedback->img_idx, camera_feedback->lat, camera_feedback->lng, camera_feedback->alt_msl, camera_feedback->alt_rel, camera_feedback->roll, camera_feedback->pitch, camera_feedback->yaw, camera_feedback->foc_len, camera_feedback->flags); +} + +/** + * @brief Send a camera_feedback message + * @param chan MAVLink channel to send the message + * + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt_msl Altitude Absolute (meters AMSL) + * @param alt_rel Altitude Relative (meters above HOME location) + * @param roll Camera Roll angle (earth frame, degrees, +-180) + * @param pitch Camera Pitch angle (earth frame, degrees, +-180) + * @param yaw Camera Yaw (earth frame, degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_feedback_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt_msl, float alt_rel, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt_msl); + _mav_put_float(buf, 20, alt_rel); + _mav_put_float(buf, 24, roll); + _mav_put_float(buf, 28, pitch); + _mav_put_float(buf, 32, yaw); + _mav_put_float(buf, 36, foc_len); + _mav_put_uint16_t(buf, 40, img_idx); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, cam_idx); + _mav_put_uint8_t(buf, 44, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt_msl = alt_msl; + packet.alt_rel = alt_rel; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_feedback_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt_msl, float alt_rel, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt_msl); + _mav_put_float(buf, 20, alt_rel); + _mav_put_float(buf, 24, roll); + _mav_put_float(buf, 28, pitch); + _mav_put_float(buf, 32, yaw); + _mav_put_float(buf, 36, foc_len); + _mav_put_uint16_t(buf, 40, img_idx); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, cam_idx); + _mav_put_uint8_t(buf, 44, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#else + mavlink_camera_feedback_t *packet = (mavlink_camera_feedback_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lng = lng; + packet->alt_msl = alt_msl; + packet->alt_rel = alt_rel; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->foc_len = foc_len; + packet->img_idx = img_idx; + packet->target_system = target_system; + packet->cam_idx = cam_idx; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_FEEDBACK UNPACKING + + +/** + * @brief Get field time_usec from camera_feedback message + * + * @return Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) + */ +static inline uint64_t mavlink_msg_camera_feedback_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field target_system from camera_feedback message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_camera_feedback_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field cam_idx from camera_feedback message + * + * @return Camera ID + */ +static inline uint8_t mavlink_msg_camera_feedback_get_cam_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field img_idx from camera_feedback message + * + * @return Image index + */ +static inline uint16_t mavlink_msg_camera_feedback_get_img_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 40); +} + +/** + * @brief Get field lat from camera_feedback message + * + * @return Latitude in (deg * 1E7) + */ +static inline int32_t mavlink_msg_camera_feedback_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lng from camera_feedback message + * + * @return Longitude in (deg * 1E7) + */ +static inline int32_t mavlink_msg_camera_feedback_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt_msl from camera_feedback message + * + * @return Altitude Absolute (meters AMSL) + */ +static inline float mavlink_msg_camera_feedback_get_alt_msl(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field alt_rel from camera_feedback message + * + * @return Altitude Relative (meters above HOME location) + */ +static inline float mavlink_msg_camera_feedback_get_alt_rel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field roll from camera_feedback message + * + * @return Camera Roll angle (earth frame, degrees, +-180) + */ +static inline float mavlink_msg_camera_feedback_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field pitch from camera_feedback message + * + * @return Camera Pitch angle (earth frame, degrees, +-180) + */ +static inline float mavlink_msg_camera_feedback_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field yaw from camera_feedback message + * + * @return Camera Yaw (earth frame, degrees, 0-360, true) + */ +static inline float mavlink_msg_camera_feedback_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field foc_len from camera_feedback message + * + * @return Focal Length (mm) + */ +static inline float mavlink_msg_camera_feedback_get_foc_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field flags from camera_feedback message + * + * @return See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + */ +static inline uint8_t mavlink_msg_camera_feedback_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 44); +} + +/** + * @brief Decode a camera_feedback message into a struct + * + * @param msg The message to decode + * @param camera_feedback C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_feedback_decode(const mavlink_message_t* msg, mavlink_camera_feedback_t* camera_feedback) +{ +#if MAVLINK_NEED_BYTE_SWAP + camera_feedback->time_usec = mavlink_msg_camera_feedback_get_time_usec(msg); + camera_feedback->lat = mavlink_msg_camera_feedback_get_lat(msg); + camera_feedback->lng = mavlink_msg_camera_feedback_get_lng(msg); + camera_feedback->alt_msl = mavlink_msg_camera_feedback_get_alt_msl(msg); + camera_feedback->alt_rel = mavlink_msg_camera_feedback_get_alt_rel(msg); + camera_feedback->roll = mavlink_msg_camera_feedback_get_roll(msg); + camera_feedback->pitch = mavlink_msg_camera_feedback_get_pitch(msg); + camera_feedback->yaw = mavlink_msg_camera_feedback_get_yaw(msg); + camera_feedback->foc_len = mavlink_msg_camera_feedback_get_foc_len(msg); + camera_feedback->img_idx = mavlink_msg_camera_feedback_get_img_idx(msg); + camera_feedback->target_system = mavlink_msg_camera_feedback_get_target_system(msg); + camera_feedback->cam_idx = mavlink_msg_camera_feedback_get_cam_idx(msg); + camera_feedback->flags = mavlink_msg_camera_feedback_get_flags(msg); +#else + memcpy(camera_feedback, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_status.h new file mode 100644 index 0000000..9d1e54e --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_camera_status.h @@ -0,0 +1,401 @@ +// MESSAGE CAMERA_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_STATUS 179 + +typedef struct __mavlink_camera_status_t +{ + uint64_t time_usec; ///< Image timestamp (microseconds since UNIX epoch, according to camera clock) + float p1; ///< Parameter 1 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + float p2; ///< Parameter 2 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + float p3; ///< Parameter 3 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + float p4; ///< Parameter 4 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + uint16_t img_idx; ///< Image index + uint8_t target_system; ///< System ID + uint8_t cam_idx; ///< Camera ID + uint8_t event_id; ///< See CAMERA_STATUS_TYPES enum for definition of the bitmask +} mavlink_camera_status_t; + +#define MAVLINK_MSG_ID_CAMERA_STATUS_LEN 29 +#define MAVLINK_MSG_ID_179_LEN 29 + +#define MAVLINK_MSG_ID_CAMERA_STATUS_CRC 189 +#define MAVLINK_MSG_ID_179_CRC 189 + + + +#define MAVLINK_MESSAGE_INFO_CAMERA_STATUS { \ + "CAMERA_STATUS", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_status_t, time_usec) }, \ + { "p1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_status_t, p1) }, \ + { "p2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_status_t, p2) }, \ + { "p3", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_status_t, p3) }, \ + { "p4", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_status_t, p4) }, \ + { "img_idx", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_status_t, img_idx) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_camera_status_t, target_system) }, \ + { "cam_idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_camera_status_t, cam_idx) }, \ + { "event_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_status_t, event_id) }, \ + } \ +} + + +/** + * @brief Pack a camera_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_STATUS_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#else + mavlink_camera_status_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +} + +/** + * @brief Pack a camera_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_STATUS_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t target_system,uint8_t cam_idx,uint16_t img_idx,uint8_t event_id,float p1,float p2,float p3,float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#else + mavlink_camera_status_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +} + +/** + * @brief Encode a camera_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_status_t* camera_status) +{ + return mavlink_msg_camera_status_pack(system_id, component_id, msg, camera_status->time_usec, camera_status->target_system, camera_status->cam_idx, camera_status->img_idx, camera_status->event_id, camera_status->p1, camera_status->p2, camera_status->p3, camera_status->p4); +} + +/** + * @brief Encode a camera_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_status_t* camera_status) +{ + return mavlink_msg_camera_status_pack_chan(system_id, component_id, chan, msg, camera_status->time_usec, camera_status->target_system, camera_status->cam_idx, camera_status->img_idx, camera_status->event_id, camera_status->p1, camera_status->p2, camera_status->p3, camera_status->p4); +} + +/** + * @brief Send a camera_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_STATUS_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_status_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +#else + mavlink_camera_status_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, buf, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +#else + mavlink_camera_status_t *packet = (mavlink_camera_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->p1 = p1; + packet->p2 = p2; + packet->p3 = p3; + packet->p4 = p4; + packet->img_idx = img_idx; + packet->target_system = target_system; + packet->cam_idx = cam_idx; + packet->event_id = event_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_STATUS UNPACKING + + +/** + * @brief Get field time_usec from camera_status message + * + * @return Image timestamp (microseconds since UNIX epoch, according to camera clock) + */ +static inline uint64_t mavlink_msg_camera_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field target_system from camera_status message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_camera_status_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field cam_idx from camera_status message + * + * @return Camera ID + */ +static inline uint8_t mavlink_msg_camera_status_get_cam_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field img_idx from camera_status message + * + * @return Image index + */ +static inline uint16_t mavlink_msg_camera_status_get_img_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field event_id from camera_status message + * + * @return See CAMERA_STATUS_TYPES enum for definition of the bitmask + */ +static inline uint8_t mavlink_msg_camera_status_get_event_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field p1 from camera_status message + * + * @return Parameter 1 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + */ +static inline float mavlink_msg_camera_status_get_p1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field p2 from camera_status message + * + * @return Parameter 2 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + */ +static inline float mavlink_msg_camera_status_get_p2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field p3 from camera_status message + * + * @return Parameter 3 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + */ +static inline float mavlink_msg_camera_status_get_p3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field p4 from camera_status message + * + * @return Parameter 4 (meaning depends on event, see CAMERA_STATUS_TYPES enum) + */ +static inline float mavlink_msg_camera_status_get_p4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a camera_status message into a struct + * + * @param msg The message to decode + * @param camera_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_status_decode(const mavlink_message_t* msg, mavlink_camera_status_t* camera_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + camera_status->time_usec = mavlink_msg_camera_status_get_time_usec(msg); + camera_status->p1 = mavlink_msg_camera_status_get_p1(msg); + camera_status->p2 = mavlink_msg_camera_status_get_p2(msg); + camera_status->p3 = mavlink_msg_camera_status_get_p3(msg); + camera_status->p4 = mavlink_msg_camera_status_get_p4(msg); + camera_status->img_idx = mavlink_msg_camera_status_get_img_idx(msg); + camera_status->target_system = mavlink_msg_camera_status_get_target_system(msg); + camera_status->cam_idx = mavlink_msg_camera_status_get_cam_idx(msg); + camera_status->event_id = mavlink_msg_camera_status_get_event_id(msg); +#else + memcpy(camera_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CAMERA_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_compassmot_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_compassmot_status.h new file mode 100644 index 0000000..9655459 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_compassmot_status.h @@ -0,0 +1,329 @@ +// MESSAGE COMPASSMOT_STATUS PACKING + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS 177 + +typedef struct __mavlink_compassmot_status_t +{ + float current; ///< current (amps) + float CompensationX; ///< Motor Compensation X + float CompensationY; ///< Motor Compensation Y + float CompensationZ; ///< Motor Compensation Z + uint16_t throttle; ///< throttle (percent*10) + uint16_t interference; ///< interference (percent) +} mavlink_compassmot_status_t; + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN 20 +#define MAVLINK_MSG_ID_177_LEN 20 + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC 240 +#define MAVLINK_MSG_ID_177_CRC 240 + + + +#define MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS { \ + "COMPASSMOT_STATUS", \ + 6, \ + { { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_compassmot_status_t, current) }, \ + { "CompensationX", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_compassmot_status_t, CompensationX) }, \ + { "CompensationY", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_compassmot_status_t, CompensationY) }, \ + { "CompensationZ", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_compassmot_status_t, CompensationZ) }, \ + { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_compassmot_status_t, throttle) }, \ + { "interference", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_compassmot_status_t, interference) }, \ + } \ +} + + +/** + * @brief Pack a compassmot_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Pack a compassmot_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t throttle,float current,uint16_t interference,float CompensationX,float CompensationY,float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Encode a compassmot_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack(system_id, component_id, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Encode a compassmot_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack_chan(system_id, component_id, chan, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Send a compassmot_status message + * @param chan MAVLink channel to send the message + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_compassmot_status_send(mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_compassmot_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t *packet = (mavlink_compassmot_status_t *)msgbuf; + packet->current = current; + packet->CompensationX = CompensationX; + packet->CompensationY = CompensationY; + packet->CompensationZ = CompensationZ; + packet->throttle = throttle; + packet->interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMPASSMOT_STATUS UNPACKING + + +/** + * @brief Get field throttle from compassmot_status message + * + * @return throttle (percent*10) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field current from compassmot_status message + * + * @return current (amps) + */ +static inline float mavlink_msg_compassmot_status_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field interference from compassmot_status message + * + * @return interference (percent) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_interference(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field CompensationX from compassmot_status message + * + * @return Motor Compensation X + */ +static inline float mavlink_msg_compassmot_status_get_CompensationX(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field CompensationY from compassmot_status message + * + * @return Motor Compensation Y + */ +static inline float mavlink_msg_compassmot_status_get_CompensationY(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field CompensationZ from compassmot_status message + * + * @return Motor Compensation Z + */ +static inline float mavlink_msg_compassmot_status_get_CompensationZ(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a compassmot_status message into a struct + * + * @param msg The message to decode + * @param compassmot_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_compassmot_status_decode(const mavlink_message_t* msg, mavlink_compassmot_status_t* compassmot_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + compassmot_status->current = mavlink_msg_compassmot_status_get_current(msg); + compassmot_status->CompensationX = mavlink_msg_compassmot_status_get_CompensationX(msg); + compassmot_status->CompensationY = mavlink_msg_compassmot_status_get_CompensationY(msg); + compassmot_status->CompensationZ = mavlink_msg_compassmot_status_get_CompensationZ(msg); + compassmot_status->throttle = mavlink_msg_compassmot_status_get_throttle(msg); + compassmot_status->interference = mavlink_msg_compassmot_status_get_interference(msg); +#else + memcpy(compassmot_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data16.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data16.h new file mode 100644 index 0000000..7bad1b2 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data16.h @@ -0,0 +1,249 @@ +// MESSAGE DATA16 PACKING + +#define MAVLINK_MSG_ID_DATA16 169 + +typedef struct __mavlink_data16_t +{ + uint8_t type; ///< data type + uint8_t len; ///< data length + uint8_t data[16]; ///< raw data +} mavlink_data16_t; + +#define MAVLINK_MSG_ID_DATA16_LEN 18 +#define MAVLINK_MSG_ID_169_LEN 18 + +#define MAVLINK_MSG_ID_DATA16_CRC 234 +#define MAVLINK_MSG_ID_169_CRC 234 + +#define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16 + +#define MAVLINK_MESSAGE_INFO_DATA16 { \ + "DATA16", \ + 3, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \ + } \ +} + + +/** + * @brief Pack a data16 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA16_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); +#else + mavlink_data16_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA16; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN); +#endif +} + +/** + * @brief Pack a data16 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA16_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); +#else + mavlink_data16_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA16; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN); +#endif +} + +/** + * @brief Encode a data16 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16) +{ + return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data); +} + +/** + * @brief Encode a data16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) +{ + return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); +} + +/** + * @brief Send a data16 message + * @param chan MAVLink channel to send the message + * + * @param type data type + * @param len data length + * @param data raw data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA16_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#else + mavlink_data16_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#else + mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA16 UNPACKING + + +/** + * @brief Get field type from data16 message + * + * @return data type + */ +static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field len from data16 message + * + * @return data length + */ +static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field data from data16 message + * + * @return raw data + */ +static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 16, 2); +} + +/** + * @brief Decode a data16 message into a struct + * + * @param msg The message to decode + * @param data16 C-struct to decode the message contents into + */ +static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16) +{ +#if MAVLINK_NEED_BYTE_SWAP + data16->type = mavlink_msg_data16_get_type(msg); + data16->len = mavlink_msg_data16_get_len(msg); + mavlink_msg_data16_get_data(msg, data16->data); +#else + memcpy(data16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA16_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data32.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data32.h new file mode 100644 index 0000000..df7a2ec --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data32.h @@ -0,0 +1,249 @@ +// MESSAGE DATA32 PACKING + +#define MAVLINK_MSG_ID_DATA32 170 + +typedef struct __mavlink_data32_t +{ + uint8_t type; ///< data type + uint8_t len; ///< data length + uint8_t data[32]; ///< raw data +} mavlink_data32_t; + +#define MAVLINK_MSG_ID_DATA32_LEN 34 +#define MAVLINK_MSG_ID_170_LEN 34 + +#define MAVLINK_MSG_ID_DATA32_CRC 73 +#define MAVLINK_MSG_ID_170_CRC 73 + +#define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32 + +#define MAVLINK_MESSAGE_INFO_DATA32 { \ + "DATA32", \ + 3, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \ + } \ +} + + +/** + * @brief Pack a data32 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA32_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); +#else + mavlink_data32_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA32; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN); +#endif +} + +/** + * @brief Pack a data32 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA32_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); +#else + mavlink_data32_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA32; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN); +#endif +} + +/** + * @brief Encode a data32 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data32 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32) +{ + return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data); +} + +/** + * @brief Encode a data32 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data32 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) +{ + return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); +} + +/** + * @brief Send a data32 message + * @param chan MAVLink channel to send the message + * + * @param type data type + * @param len data length + * @param data raw data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA32_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#else + mavlink_data32_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#else + mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA32 UNPACKING + + +/** + * @brief Get field type from data32 message + * + * @return data type + */ +static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field len from data32 message + * + * @return data length + */ +static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field data from data32 message + * + * @return raw data + */ +static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 32, 2); +} + +/** + * @brief Decode a data32 message into a struct + * + * @param msg The message to decode + * @param data32 C-struct to decode the message contents into + */ +static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32) +{ +#if MAVLINK_NEED_BYTE_SWAP + data32->type = mavlink_msg_data32_get_type(msg); + data32->len = mavlink_msg_data32_get_len(msg); + mavlink_msg_data32_get_data(msg, data32->data); +#else + memcpy(data32, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA32_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data64.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data64.h new file mode 100644 index 0000000..c354f89 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data64.h @@ -0,0 +1,249 @@ +// MESSAGE DATA64 PACKING + +#define MAVLINK_MSG_ID_DATA64 171 + +typedef struct __mavlink_data64_t +{ + uint8_t type; ///< data type + uint8_t len; ///< data length + uint8_t data[64]; ///< raw data +} mavlink_data64_t; + +#define MAVLINK_MSG_ID_DATA64_LEN 66 +#define MAVLINK_MSG_ID_171_LEN 66 + +#define MAVLINK_MSG_ID_DATA64_CRC 181 +#define MAVLINK_MSG_ID_171_CRC 181 + +#define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64 + +#define MAVLINK_MESSAGE_INFO_DATA64 { \ + "DATA64", \ + 3, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \ + } \ +} + + +/** + * @brief Pack a data64 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA64_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); +#else + mavlink_data64_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA64; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN); +#endif +} + +/** + * @brief Pack a data64 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA64_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); +#else + mavlink_data64_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA64; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN); +#endif +} + +/** + * @brief Encode a data64 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data64 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64) +{ + return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data); +} + +/** + * @brief Encode a data64 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data64 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) +{ + return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); +} + +/** + * @brief Send a data64 message + * @param chan MAVLink channel to send the message + * + * @param type data type + * @param len data length + * @param data raw data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA64_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#else + mavlink_data64_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#else + mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA64 UNPACKING + + +/** + * @brief Get field type from data64 message + * + * @return data type + */ +static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field len from data64 message + * + * @return data length + */ +static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field data from data64 message + * + * @return raw data + */ +static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 64, 2); +} + +/** + * @brief Decode a data64 message into a struct + * + * @param msg The message to decode + * @param data64 C-struct to decode the message contents into + */ +static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64) +{ +#if MAVLINK_NEED_BYTE_SWAP + data64->type = mavlink_msg_data64_get_type(msg); + data64->len = mavlink_msg_data64_get_len(msg); + mavlink_msg_data64_get_data(msg, data64->data); +#else + memcpy(data64, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA64_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data96.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data96.h new file mode 100644 index 0000000..634e3f8 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_data96.h @@ -0,0 +1,249 @@ +// MESSAGE DATA96 PACKING + +#define MAVLINK_MSG_ID_DATA96 172 + +typedef struct __mavlink_data96_t +{ + uint8_t type; ///< data type + uint8_t len; ///< data length + uint8_t data[96]; ///< raw data +} mavlink_data96_t; + +#define MAVLINK_MSG_ID_DATA96_LEN 98 +#define MAVLINK_MSG_ID_172_LEN 98 + +#define MAVLINK_MSG_ID_DATA96_CRC 22 +#define MAVLINK_MSG_ID_172_CRC 22 + +#define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96 + +#define MAVLINK_MESSAGE_INFO_DATA96 { \ + "DATA96", \ + 3, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \ + } \ +} + + +/** + * @brief Pack a data96 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA96_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); +#else + mavlink_data96_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA96; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN); +#endif +} + +/** + * @brief Pack a data96 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type data type + * @param len data length + * @param data raw data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA96_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); +#else + mavlink_data96_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA96; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN); +#endif +} + +/** + * @brief Encode a data96 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data96 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96) +{ + return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data); +} + +/** + * @brief Encode a data96 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data96 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) +{ + return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); +} + +/** + * @brief Send a data96 message + * @param chan MAVLink channel to send the message + * + * @param type data type + * @param len data length + * @param data raw data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA96_LEN]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#else + mavlink_data96_t packet; + packet.type = type; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#else + mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA96 UNPACKING + + +/** + * @brief Get field type from data96 message + * + * @return data type + */ +static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field len from data96 message + * + * @return data length + */ +static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field data from data96 message + * + * @return raw data + */ +static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 96, 2); +} + +/** + * @brief Decode a data96 message into a struct + * + * @param msg The message to decode + * @param data96 C-struct to decode the message contents into + */ +static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96) +{ +#if MAVLINK_NEED_BYTE_SWAP + data96->type = mavlink_msg_data96_get_type(msg); + data96->len = mavlink_msg_data96_get_len(msg); + mavlink_msg_data96_get_data(msg, data96->data); +#else + memcpy(data96, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA96_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_configure.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_configure.h new file mode 100644 index 0000000..d5f9e71 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_configure.h @@ -0,0 +1,449 @@ +// MESSAGE DIGICAM_CONFIGURE PACKING + +#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154 + +typedef struct __mavlink_digicam_configure_t +{ + float extra_value; ///< Correspondent value to given extra_param + uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore) + uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) +} mavlink_digicam_configure_t; + +#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15 +#define MAVLINK_MSG_ID_154_LEN 15 + +#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC 84 +#define MAVLINK_MSG_ID_154_CRC 84 + + + +#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \ + "DIGICAM_CONFIGURE", \ + 11, \ + { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_configure_t, extra_value) }, \ + { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, target_component) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, mode) }, \ + { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, aperture) }, \ + { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, iso) }, \ + { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_configure_t, exposure_type) }, \ + { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_configure_t, command_id) }, \ + { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \ + { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_digicam_configure_t, extra_param) }, \ + } \ +} + + +/** + * @brief Pack a digicam_configure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#else + mavlink_digicam_configure_t packet; + packet.extra_value = extra_value; + packet.shutter_speed = shutter_speed; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.aperture = aperture; + packet.iso = iso; + packet.exposure_type = exposure_type; + packet.command_id = command_id; + packet.engine_cut_off = engine_cut_off; + packet.extra_param = extra_param; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +} + +/** + * @brief Pack a digicam_configure message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t mode,uint16_t shutter_speed,uint8_t aperture,uint8_t iso,uint8_t exposure_type,uint8_t command_id,uint8_t engine_cut_off,uint8_t extra_param,float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#else + mavlink_digicam_configure_t packet; + packet.extra_value = extra_value; + packet.shutter_speed = shutter_speed; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.aperture = aperture; + packet.iso = iso; + packet.exposure_type = exposure_type; + packet.command_id = command_id; + packet.engine_cut_off = engine_cut_off; + packet.extra_param = extra_param; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +} + +/** + * @brief Encode a digicam_configure struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param digicam_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) +{ + return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); +} + +/** + * @brief Encode a digicam_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) +{ + return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); +} + +/** + * @brief Send a digicam_configure message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#else + mavlink_digicam_configure_t packet; + packet.extra_value = extra_value; + packet.shutter_speed = shutter_speed; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mode = mode; + packet.aperture = aperture; + packet.iso = iso; + packet.exposure_type = exposure_type; + packet.command_id = command_id; + packet.engine_cut_off = engine_cut_off; + packet.extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#else + mavlink_digicam_configure_t *packet = (mavlink_digicam_configure_t *)msgbuf; + packet->extra_value = extra_value; + packet->shutter_speed = shutter_speed; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mode = mode; + packet->aperture = aperture; + packet->iso = iso; + packet->exposure_type = exposure_type; + packet->command_id = command_id; + packet->engine_cut_off = engine_cut_off; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DIGICAM_CONFIGURE UNPACKING + + +/** + * @brief Get field target_system from digicam_configure message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field target_component from digicam_configure message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field mode from digicam_configure message + * + * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field shutter_speed from digicam_configure message + * + * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + */ +static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field aperture from digicam_configure message + * + * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field iso from digicam_configure message + * + * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field exposure_type from digicam_configure message + * + * @return Exposure type enumeration from 1 to N (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field command_id from digicam_configure message + * + * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + */ +static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field engine_cut_off from digicam_configure message + * + * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field extra_param from digicam_configure message + * + * @return Extra parameters enumeration (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field extra_value from digicam_configure message + * + * @return Correspondent value to given extra_param + */ +static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Decode a digicam_configure message into a struct + * + * @param msg The message to decode + * @param digicam_configure C-struct to decode the message contents into + */ +static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure) +{ +#if MAVLINK_NEED_BYTE_SWAP + digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg); + digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg); + digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg); + digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg); + digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg); + digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg); + digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg); + digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg); + digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg); + digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg); + digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg); +#else + memcpy(digicam_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_control.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_control.h new file mode 100644 index 0000000..68484f7 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_digicam_control.h @@ -0,0 +1,425 @@ +// MESSAGE DIGICAM_CONTROL PACKING + +#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155 + +typedef struct __mavlink_digicam_control_t +{ + float extra_value; ///< Correspondent value to given extra_param + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore) + int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position + uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + uint8_t shot; ///< 0: ignore, 1: shot or start filming + uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) +} mavlink_digicam_control_t; + +#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13 +#define MAVLINK_MSG_ID_155_LEN 13 + +#define MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC 22 +#define MAVLINK_MSG_ID_155_CRC 22 + + + +#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \ + "DIGICAM_CONTROL", \ + 10, \ + { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_control_t, extra_value) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_digicam_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, target_component) }, \ + { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, session) }, \ + { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, zoom_pos) }, \ + { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 8, offsetof(mavlink_digicam_control_t, zoom_step) }, \ + { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_control_t, focus_lock) }, \ + { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_control_t, shot) }, \ + { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_control_t, command_id) }, \ + { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_control_t, extra_param) }, \ + } \ +} + + +/** + * @brief Pack a digicam_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) + * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position + * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + * @param shot 0: ignore, 1: shot or start filming + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#else + mavlink_digicam_control_t packet; + packet.extra_value = extra_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.session = session; + packet.zoom_pos = zoom_pos; + packet.zoom_step = zoom_step; + packet.focus_lock = focus_lock; + packet.shot = shot; + packet.command_id = command_id; + packet.extra_param = extra_param; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a digicam_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) + * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position + * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + * @param shot 0: ignore, 1: shot or start filming + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t session,uint8_t zoom_pos,int8_t zoom_step,uint8_t focus_lock,uint8_t shot,uint8_t command_id,uint8_t extra_param,float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#else + mavlink_digicam_control_t packet; + packet.extra_value = extra_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.session = session; + packet.zoom_pos = zoom_pos; + packet.zoom_step = zoom_step; + packet.focus_lock = focus_lock; + packet.shot = shot; + packet.command_id = command_id; + packet.extra_param = extra_param; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a digicam_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param digicam_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) +{ + return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); +} + +/** + * @brief Encode a digicam_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) +{ + return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); +} + +/** + * @brief Send a digicam_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) + * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position + * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + * @param shot 0: ignore, 1: shot or start filming + * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + * @param extra_param Extra parameters enumeration (0 means ignore) + * @param extra_value Correspondent value to given extra_param + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#else + mavlink_digicam_control_t packet; + packet.extra_value = extra_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.session = session; + packet.zoom_pos = zoom_pos; + packet.zoom_step = zoom_step; + packet.focus_lock = focus_lock; + packet.shot = shot; + packet.command_id = command_id; + packet.extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#else + mavlink_digicam_control_t *packet = (mavlink_digicam_control_t *)msgbuf; + packet->extra_value = extra_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->session = session; + packet->zoom_pos = zoom_pos; + packet->zoom_step = zoom_step; + packet->focus_lock = focus_lock; + packet->shot = shot; + packet->command_id = command_id; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DIGICAM_CONTROL UNPACKING + + +/** + * @brief Get field target_system from digicam_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from digicam_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field session from digicam_control message + * + * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + */ +static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field zoom_pos from digicam_control message + * + * @return 1 to N //Zoom's absolute position (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field zoom_step from digicam_control message + * + * @return -100 to 100 //Zooming step value to offset zoom from the current position + */ +static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 8); +} + +/** + * @brief Get field focus_lock from digicam_control message + * + * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + */ +static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field shot from digicam_control message + * + * @return 0: ignore, 1: shot or start filming + */ +static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field command_id from digicam_control message + * + * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + */ +static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field extra_param from digicam_control message + * + * @return Extra parameters enumeration (0 means ignore) + */ +static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field extra_value from digicam_control message + * + * @return Correspondent value to given extra_param + */ +static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Decode a digicam_control message into a struct + * + * @param msg The message to decode + * @param digicam_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg); + digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg); + digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg); + digicam_control->session = mavlink_msg_digicam_control_get_session(msg); + digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg); + digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg); + digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg); + digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg); + digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg); + digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg); +#else + memcpy(digicam_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ekf_status_report.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ekf_status_report.h new file mode 100644 index 0000000..31ff548 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ekf_status_report.h @@ -0,0 +1,329 @@ +// MESSAGE EKF_STATUS_REPORT PACKING + +#define MAVLINK_MSG_ID_EKF_STATUS_REPORT 193 + +typedef struct __mavlink_ekf_status_report_t +{ + float velocity_variance; ///< Velocity variance + float pos_horiz_variance; ///< Horizontal Position variance + float pos_vert_variance; ///< Vertical Position variance + float compass_variance; ///< Compass variance + float terrain_alt_variance; ///< Terrain Altitude variance + uint16_t flags; ///< Flags +} mavlink_ekf_status_report_t; + +#define MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN 22 +#define MAVLINK_MSG_ID_193_LEN 22 + +#define MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC 71 +#define MAVLINK_MSG_ID_193_CRC 71 + + + +#define MAVLINK_MESSAGE_INFO_EKF_STATUS_REPORT { \ + "EKF_STATUS_REPORT", \ + 6, \ + { { "velocity_variance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ekf_status_report_t, velocity_variance) }, \ + { "pos_horiz_variance", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ekf_status_report_t, pos_horiz_variance) }, \ + { "pos_vert_variance", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ekf_status_report_t, pos_vert_variance) }, \ + { "compass_variance", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ekf_status_report_t, compass_variance) }, \ + { "terrain_alt_variance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ekf_status_report_t, terrain_alt_variance) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_ekf_status_report_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a ekf_status_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param flags Flags + * @param velocity_variance Velocity variance + * @param pos_horiz_variance Horizontal Position variance + * @param pos_vert_variance Vertical Position variance + * @param compass_variance Compass variance + * @param terrain_alt_variance Terrain Altitude variance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ekf_status_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t flags, float velocity_variance, float pos_horiz_variance, float pos_vert_variance, float compass_variance, float terrain_alt_variance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN]; + _mav_put_float(buf, 0, velocity_variance); + _mav_put_float(buf, 4, pos_horiz_variance); + _mav_put_float(buf, 8, pos_vert_variance); + _mav_put_float(buf, 12, compass_variance); + _mav_put_float(buf, 16, terrain_alt_variance); + _mav_put_uint16_t(buf, 20, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#else + mavlink_ekf_status_report_t packet; + packet.velocity_variance = velocity_variance; + packet.pos_horiz_variance = pos_horiz_variance; + packet.pos_vert_variance = pos_vert_variance; + packet.compass_variance = compass_variance; + packet.terrain_alt_variance = terrain_alt_variance; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EKF_STATUS_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +} + +/** + * @brief Pack a ekf_status_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flags Flags + * @param velocity_variance Velocity variance + * @param pos_horiz_variance Horizontal Position variance + * @param pos_vert_variance Vertical Position variance + * @param compass_variance Compass variance + * @param terrain_alt_variance Terrain Altitude variance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ekf_status_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t flags,float velocity_variance,float pos_horiz_variance,float pos_vert_variance,float compass_variance,float terrain_alt_variance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN]; + _mav_put_float(buf, 0, velocity_variance); + _mav_put_float(buf, 4, pos_horiz_variance); + _mav_put_float(buf, 8, pos_vert_variance); + _mav_put_float(buf, 12, compass_variance); + _mav_put_float(buf, 16, terrain_alt_variance); + _mav_put_uint16_t(buf, 20, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#else + mavlink_ekf_status_report_t packet; + packet.velocity_variance = velocity_variance; + packet.pos_horiz_variance = pos_horiz_variance; + packet.pos_vert_variance = pos_vert_variance; + packet.compass_variance = compass_variance; + packet.terrain_alt_variance = terrain_alt_variance; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EKF_STATUS_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +} + +/** + * @brief Encode a ekf_status_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ekf_status_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ekf_status_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ekf_status_report_t* ekf_status_report) +{ + return mavlink_msg_ekf_status_report_pack(system_id, component_id, msg, ekf_status_report->flags, ekf_status_report->velocity_variance, ekf_status_report->pos_horiz_variance, ekf_status_report->pos_vert_variance, ekf_status_report->compass_variance, ekf_status_report->terrain_alt_variance); +} + +/** + * @brief Encode a ekf_status_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ekf_status_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ekf_status_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ekf_status_report_t* ekf_status_report) +{ + return mavlink_msg_ekf_status_report_pack_chan(system_id, component_id, chan, msg, ekf_status_report->flags, ekf_status_report->velocity_variance, ekf_status_report->pos_horiz_variance, ekf_status_report->pos_vert_variance, ekf_status_report->compass_variance, ekf_status_report->terrain_alt_variance); +} + +/** + * @brief Send a ekf_status_report message + * @param chan MAVLink channel to send the message + * + * @param flags Flags + * @param velocity_variance Velocity variance + * @param pos_horiz_variance Horizontal Position variance + * @param pos_vert_variance Vertical Position variance + * @param compass_variance Compass variance + * @param terrain_alt_variance Terrain Altitude variance + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ekf_status_report_send(mavlink_channel_t chan, uint16_t flags, float velocity_variance, float pos_horiz_variance, float pos_vert_variance, float compass_variance, float terrain_alt_variance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN]; + _mav_put_float(buf, 0, velocity_variance); + _mav_put_float(buf, 4, pos_horiz_variance); + _mav_put_float(buf, 8, pos_vert_variance); + _mav_put_float(buf, 12, compass_variance); + _mav_put_float(buf, 16, terrain_alt_variance); + _mav_put_uint16_t(buf, 20, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +#else + mavlink_ekf_status_report_t packet; + packet.velocity_variance = velocity_variance; + packet.pos_horiz_variance = pos_horiz_variance; + packet.pos_vert_variance = pos_vert_variance; + packet.compass_variance = compass_variance; + packet.terrain_alt_variance = terrain_alt_variance; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, (const char *)&packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, (const char *)&packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ekf_status_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t flags, float velocity_variance, float pos_horiz_variance, float pos_vert_variance, float compass_variance, float terrain_alt_variance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, velocity_variance); + _mav_put_float(buf, 4, pos_horiz_variance); + _mav_put_float(buf, 8, pos_vert_variance); + _mav_put_float(buf, 12, compass_variance); + _mav_put_float(buf, 16, terrain_alt_variance); + _mav_put_uint16_t(buf, 20, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, buf, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +#else + mavlink_ekf_status_report_t *packet = (mavlink_ekf_status_report_t *)msgbuf; + packet->velocity_variance = velocity_variance; + packet->pos_horiz_variance = pos_horiz_variance; + packet->pos_vert_variance = pos_vert_variance; + packet->compass_variance = compass_variance; + packet->terrain_alt_variance = terrain_alt_variance; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, (const char *)packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN, MAVLINK_MSG_ID_EKF_STATUS_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EKF_STATUS_REPORT, (const char *)packet, MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE EKF_STATUS_REPORT UNPACKING + + +/** + * @brief Get field flags from ekf_status_report message + * + * @return Flags + */ +static inline uint16_t mavlink_msg_ekf_status_report_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field velocity_variance from ekf_status_report message + * + * @return Velocity variance + */ +static inline float mavlink_msg_ekf_status_report_get_velocity_variance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pos_horiz_variance from ekf_status_report message + * + * @return Horizontal Position variance + */ +static inline float mavlink_msg_ekf_status_report_get_pos_horiz_variance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field pos_vert_variance from ekf_status_report message + * + * @return Vertical Position variance + */ +static inline float mavlink_msg_ekf_status_report_get_pos_vert_variance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field compass_variance from ekf_status_report message + * + * @return Compass variance + */ +static inline float mavlink_msg_ekf_status_report_get_compass_variance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field terrain_alt_variance from ekf_status_report message + * + * @return Terrain Altitude variance + */ +static inline float mavlink_msg_ekf_status_report_get_terrain_alt_variance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a ekf_status_report message into a struct + * + * @param msg The message to decode + * @param ekf_status_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_ekf_status_report_decode(const mavlink_message_t* msg, mavlink_ekf_status_report_t* ekf_status_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + ekf_status_report->velocity_variance = mavlink_msg_ekf_status_report_get_velocity_variance(msg); + ekf_status_report->pos_horiz_variance = mavlink_msg_ekf_status_report_get_pos_horiz_variance(msg); + ekf_status_report->pos_vert_variance = mavlink_msg_ekf_status_report_get_pos_vert_variance(msg); + ekf_status_report->compass_variance = mavlink_msg_ekf_status_report_get_compass_variance(msg); + ekf_status_report->terrain_alt_variance = mavlink_msg_ekf_status_report_get_terrain_alt_variance(msg); + ekf_status_report->flags = mavlink_msg_ekf_status_report_get_flags(msg); +#else + memcpy(ekf_status_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_EKF_STATUS_REPORT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_fetch_point.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_fetch_point.h new file mode 100644 index 0000000..4021c7a --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -0,0 +1,257 @@ +// MESSAGE FENCE_FETCH_POINT PACKING + +#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161 + +typedef struct __mavlink_fence_fetch_point_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 1, 0 is for return point) +} mavlink_fence_fetch_point_t; + +#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3 +#define MAVLINK_MSG_ID_161_LEN 3 + +#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC 68 +#define MAVLINK_MSG_ID_161_CRC 68 + + + +#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \ + "FENCE_FETCH_POINT", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \ + } \ +} + + +/** + * @brief Pack a fence_fetch_point message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#else + mavlink_fence_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Pack a fence_fetch_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#else + mavlink_fence_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Encode a fence_fetch_point struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param fence_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) +{ + return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); +} + +/** + * @brief Encode a fence_fetch_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) +{ + return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); +} + +/** + * @brief Send a fence_fetch_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#else + mavlink_fence_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#else + mavlink_fence_fetch_point_t *packet = (mavlink_fence_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE FENCE_FETCH_POINT UNPACKING + + +/** + * @brief Get field target_system from fence_fetch_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from fence_fetch_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field idx from fence_fetch_point message + * + * @return point index (first point is 1, 0 is for return point) + */ +static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a fence_fetch_point message into a struct + * + * @param msg The message to decode + * @param fence_fetch_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg); + fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg); + fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg); +#else + memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_point.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_point.h new file mode 100644 index 0000000..6fc2c83 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_point.h @@ -0,0 +1,329 @@ +// MESSAGE FENCE_POINT PACKING + +#define MAVLINK_MSG_ID_FENCE_POINT 160 + +typedef struct __mavlink_fence_point_t +{ + float lat; ///< Latitude of point + float lng; ///< Longitude of point + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 1, 0 is for return point) + uint8_t count; ///< total number of points (for sanity checking) +} mavlink_fence_point_t; + +#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12 +#define MAVLINK_MSG_ID_160_LEN 12 + +#define MAVLINK_MSG_ID_FENCE_POINT_CRC 78 +#define MAVLINK_MSG_ID_160_CRC 78 + + + +#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \ + "FENCE_POINT", \ + 6, \ + { { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fence_point_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lng) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fence_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_fence_point_t, idx) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_fence_point_t, count) }, \ + } \ +} + + +/** + * @brief Pack a fence_point message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#else + mavlink_fence_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +} + +/** + * @brief Pack a fence_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,float lat,float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#else + mavlink_fence_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +} + +/** + * @brief Encode a fence_point struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param fence_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) +{ + return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); +} + +/** + * @brief Encode a fence_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) +{ + return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); +} + +/** + * @brief Send a fence_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#else + mavlink_fence_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_FENCE_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#else + mavlink_fence_point_t *packet = (mavlink_fence_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE FENCE_POINT UNPACKING + + +/** + * @brief Get field target_system from fence_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field target_component from fence_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field idx from fence_point message + * + * @return point index (first point is 1, 0 is for return point) + */ +static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field count from fence_point message + * + * @return total number of points (for sanity checking) + */ +static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field lat from fence_point message + * + * @return Latitude of point + */ +static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field lng from fence_point message + * + * @return Longitude of point + */ +static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Decode a fence_point message into a struct + * + * @param msg The message to decode + * @param fence_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + fence_point->lat = mavlink_msg_fence_point_get_lat(msg); + fence_point->lng = mavlink_msg_fence_point_get_lng(msg); + fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg); + fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg); + fence_point->idx = mavlink_msg_fence_point_get_idx(msg); + fence_point->count = mavlink_msg_fence_point_get_count(msg); +#else + memcpy(fence_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_status.h new file mode 100644 index 0000000..c50d377 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_fence_status.h @@ -0,0 +1,281 @@ +// MESSAGE FENCE_STATUS PACKING + +#define MAVLINK_MSG_ID_FENCE_STATUS 162 + +typedef struct __mavlink_fence_status_t +{ + uint32_t breach_time; ///< time of last breach in milliseconds since boot + uint16_t breach_count; ///< number of fence breaches + uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside + uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum) +} mavlink_fence_status_t; + +#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8 +#define MAVLINK_MSG_ID_162_LEN 8 + +#define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189 +#define MAVLINK_MSG_ID_162_CRC 189 + + + +#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ + "FENCE_STATUS", \ + 4, \ + { { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ + { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ + { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ + { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ + } \ +} + + +/** + * @brief Pack a fence_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param breach_status 0 if currently inside fence, 1 if outside + * @param breach_count number of fence breaches + * @param breach_type last breach type (see FENCE_BREACH_* enum) + * @param breach_time time of last breach in milliseconds since boot + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#else + mavlink_fence_status_t packet; + packet.breach_time = breach_time; + packet.breach_count = breach_count; + packet.breach_status = breach_status; + packet.breach_type = breach_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +} + +/** + * @brief Pack a fence_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param breach_status 0 if currently inside fence, 1 if outside + * @param breach_count number of fence breaches + * @param breach_type last breach type (see FENCE_BREACH_* enum) + * @param breach_time time of last breach in milliseconds since boot + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#else + mavlink_fence_status_t packet; + packet.breach_time = breach_time; + packet.breach_count = breach_count; + packet.breach_status = breach_status; + packet.breach_type = breach_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +} + +/** + * @brief Encode a fence_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); +} + +/** + * @brief Encode a fence_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); +} + +/** + * @brief Send a fence_status message + * @param chan MAVLink channel to send the message + * + * @param breach_status 0 if currently inside fence, 1 if outside + * @param breach_count number of fence breaches + * @param breach_type last breach type (see FENCE_BREACH_* enum) + * @param breach_time time of last breach in milliseconds since boot + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#else + mavlink_fence_status_t packet; + packet.breach_time = breach_time; + packet.breach_count = breach_count; + packet.breach_status = breach_status; + packet.breach_type = breach_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#else + mavlink_fence_status_t *packet = (mavlink_fence_status_t *)msgbuf; + packet->breach_time = breach_time; + packet->breach_count = breach_count; + packet->breach_status = breach_status; + packet->breach_type = breach_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE FENCE_STATUS UNPACKING + + +/** + * @brief Get field breach_status from fence_status message + * + * @return 0 if currently inside fence, 1 if outside + */ +static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field breach_count from fence_status message + * + * @return number of fence breaches + */ +static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field breach_type from fence_status message + * + * @return last breach type (see FENCE_BREACH_* enum) + */ +static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field breach_time from fence_status message + * + * @return time of last breach in milliseconds since boot + */ +static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Decode a fence_status message into a struct + * + * @param msg The message to decode + * @param fence_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg); + fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg); + fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg); + fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg); +#else + memcpy(fence_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_control.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_control.h new file mode 100644 index 0000000..1b52501 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_control.h @@ -0,0 +1,305 @@ +// MESSAGE GIMBAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_GIMBAL_CONTROL 201 + +typedef struct __mavlink_gimbal_control_t +{ + float demanded_rate_x; ///< Demanded angular rate X (rad/s) + float demanded_rate_y; ///< Demanded angular rate Y (rad/s) + float demanded_rate_z; ///< Demanded angular rate Z (rad/s) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_gimbal_control_t; + +#define MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN 14 +#define MAVLINK_MSG_ID_201_LEN 14 + +#define MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC 205 +#define MAVLINK_MSG_ID_201_CRC 205 + + + +#define MAVLINK_MESSAGE_INFO_GIMBAL_CONTROL { \ + "GIMBAL_CONTROL", \ + 5, \ + { { "demanded_rate_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_gimbal_control_t, demanded_rate_x) }, \ + { "demanded_rate_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_control_t, demanded_rate_y) }, \ + { "demanded_rate_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_control_t, demanded_rate_z) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gimbal_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_gimbal_control_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a gimbal_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param demanded_rate_x Demanded angular rate X (rad/s) + * @param demanded_rate_y Demanded angular rate Y (rad/s) + * @param demanded_rate_z Demanded angular rate Z (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float demanded_rate_x, float demanded_rate_y, float demanded_rate_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN]; + _mav_put_float(buf, 0, demanded_rate_x); + _mav_put_float(buf, 4, demanded_rate_y); + _mav_put_float(buf, 8, demanded_rate_z); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#else + mavlink_gimbal_control_t packet; + packet.demanded_rate_x = demanded_rate_x; + packet.demanded_rate_y = demanded_rate_y; + packet.demanded_rate_z = demanded_rate_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a gimbal_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param demanded_rate_x Demanded angular rate X (rad/s) + * @param demanded_rate_y Demanded angular rate Y (rad/s) + * @param demanded_rate_z Demanded angular rate Z (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float demanded_rate_x,float demanded_rate_y,float demanded_rate_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN]; + _mav_put_float(buf, 0, demanded_rate_x); + _mav_put_float(buf, 4, demanded_rate_y); + _mav_put_float(buf, 8, demanded_rate_z); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#else + mavlink_gimbal_control_t packet; + packet.demanded_rate_x = demanded_rate_x; + packet.demanded_rate_y = demanded_rate_y; + packet.demanded_rate_z = demanded_rate_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a gimbal_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_control_t* gimbal_control) +{ + return mavlink_msg_gimbal_control_pack(system_id, component_id, msg, gimbal_control->target_system, gimbal_control->target_component, gimbal_control->demanded_rate_x, gimbal_control->demanded_rate_y, gimbal_control->demanded_rate_z); +} + +/** + * @brief Encode a gimbal_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_control_t* gimbal_control) +{ + return mavlink_msg_gimbal_control_pack_chan(system_id, component_id, chan, msg, gimbal_control->target_system, gimbal_control->target_component, gimbal_control->demanded_rate_x, gimbal_control->demanded_rate_y, gimbal_control->demanded_rate_z); +} + +/** + * @brief Send a gimbal_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param demanded_rate_x Demanded angular rate X (rad/s) + * @param demanded_rate_y Demanded angular rate Y (rad/s) + * @param demanded_rate_z Demanded angular rate Z (rad/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float demanded_rate_x, float demanded_rate_y, float demanded_rate_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN]; + _mav_put_float(buf, 0, demanded_rate_x); + _mav_put_float(buf, 4, demanded_rate_y); + _mav_put_float(buf, 8, demanded_rate_z); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +#else + mavlink_gimbal_control_t packet; + packet.demanded_rate_x = demanded_rate_x; + packet.demanded_rate_y = demanded_rate_y; + packet.demanded_rate_z = demanded_rate_z; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float demanded_rate_x, float demanded_rate_y, float demanded_rate_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, demanded_rate_x); + _mav_put_float(buf, 4, demanded_rate_y); + _mav_put_float(buf, 8, demanded_rate_z); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +#else + mavlink_gimbal_control_t *packet = (mavlink_gimbal_control_t *)msgbuf; + packet->demanded_rate_x = demanded_rate_x; + packet->demanded_rate_y = demanded_rate_y; + packet->demanded_rate_z = demanded_rate_z; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_CONTROL UNPACKING + + +/** + * @brief Get field target_system from gimbal_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field target_component from gimbal_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field demanded_rate_x from gimbal_control message + * + * @return Demanded angular rate X (rad/s) + */ +static inline float mavlink_msg_gimbal_control_get_demanded_rate_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field demanded_rate_y from gimbal_control message + * + * @return Demanded angular rate Y (rad/s) + */ +static inline float mavlink_msg_gimbal_control_get_demanded_rate_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field demanded_rate_z from gimbal_control message + * + * @return Demanded angular rate Z (rad/s) + */ +static inline float mavlink_msg_gimbal_control_get_demanded_rate_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a gimbal_control message into a struct + * + * @param msg The message to decode + * @param gimbal_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_control_decode(const mavlink_message_t* msg, mavlink_gimbal_control_t* gimbal_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + gimbal_control->demanded_rate_x = mavlink_msg_gimbal_control_get_demanded_rate_x(msg); + gimbal_control->demanded_rate_y = mavlink_msg_gimbal_control_get_demanded_rate_y(msg); + gimbal_control->demanded_rate_z = mavlink_msg_gimbal_control_get_demanded_rate_z(msg); + gimbal_control->target_system = mavlink_msg_gimbal_control_get_target_system(msg); + gimbal_control->target_component = mavlink_msg_gimbal_control_get_target_component(msg); +#else + memcpy(gimbal_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GIMBAL_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_report.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_report.h new file mode 100644 index 0000000..6773106 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_report.h @@ -0,0 +1,473 @@ +// MESSAGE GIMBAL_REPORT PACKING + +#define MAVLINK_MSG_ID_GIMBAL_REPORT 200 + +typedef struct __mavlink_gimbal_report_t +{ + float delta_time; ///< Time since last update (seconds) + float delta_angle_x; ///< Delta angle X (radians) + float delta_angle_y; ///< Delta angle Y (radians) + float delta_angle_z; ///< Delta angle X (radians) + float delta_velocity_x; ///< Delta velocity X (m/s) + float delta_velocity_y; ///< Delta velocity Y (m/s) + float delta_velocity_z; ///< Delta velocity Z (m/s) + float joint_roll; ///< Joint ROLL (radians) + float joint_el; ///< Joint EL (radians) + float joint_az; ///< Joint AZ (radians) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_gimbal_report_t; + +#define MAVLINK_MSG_ID_GIMBAL_REPORT_LEN 42 +#define MAVLINK_MSG_ID_200_LEN 42 + +#define MAVLINK_MSG_ID_GIMBAL_REPORT_CRC 134 +#define MAVLINK_MSG_ID_200_CRC 134 + + + +#define MAVLINK_MESSAGE_INFO_GIMBAL_REPORT { \ + "GIMBAL_REPORT", \ + 12, \ + { { "delta_time", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_gimbal_report_t, delta_time) }, \ + { "delta_angle_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_report_t, delta_angle_x) }, \ + { "delta_angle_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_report_t, delta_angle_y) }, \ + { "delta_angle_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_report_t, delta_angle_z) }, \ + { "delta_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_report_t, delta_velocity_x) }, \ + { "delta_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_report_t, delta_velocity_y) }, \ + { "delta_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_report_t, delta_velocity_z) }, \ + { "joint_roll", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_report_t, joint_roll) }, \ + { "joint_el", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_report_t, joint_el) }, \ + { "joint_az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_report_t, joint_az) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_gimbal_report_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_gimbal_report_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a gimbal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param delta_time Time since last update (seconds) + * @param delta_angle_x Delta angle X (radians) + * @param delta_angle_y Delta angle Y (radians) + * @param delta_angle_z Delta angle X (radians) + * @param delta_velocity_x Delta velocity X (m/s) + * @param delta_velocity_y Delta velocity Y (m/s) + * @param delta_velocity_z Delta velocity Z (m/s) + * @param joint_roll Joint ROLL (radians) + * @param joint_el Joint EL (radians) + * @param joint_az Joint AZ (radians) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float delta_time, float delta_angle_x, float delta_angle_y, float delta_angle_z, float delta_velocity_x, float delta_velocity_y, float delta_velocity_z, float joint_roll, float joint_el, float joint_az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_REPORT_LEN]; + _mav_put_float(buf, 0, delta_time); + _mav_put_float(buf, 4, delta_angle_x); + _mav_put_float(buf, 8, delta_angle_y); + _mav_put_float(buf, 12, delta_angle_z); + _mav_put_float(buf, 16, delta_velocity_x); + _mav_put_float(buf, 20, delta_velocity_y); + _mav_put_float(buf, 24, delta_velocity_z); + _mav_put_float(buf, 28, joint_roll); + _mav_put_float(buf, 32, joint_el); + _mav_put_float(buf, 36, joint_az); + _mav_put_uint8_t(buf, 40, target_system); + _mav_put_uint8_t(buf, 41, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#else + mavlink_gimbal_report_t packet; + packet.delta_time = delta_time; + packet.delta_angle_x = delta_angle_x; + packet.delta_angle_y = delta_angle_y; + packet.delta_angle_z = delta_angle_z; + packet.delta_velocity_x = delta_velocity_x; + packet.delta_velocity_y = delta_velocity_y; + packet.delta_velocity_z = delta_velocity_z; + packet.joint_roll = joint_roll; + packet.joint_el = joint_el; + packet.joint_az = joint_az; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +} + +/** + * @brief Pack a gimbal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param delta_time Time since last update (seconds) + * @param delta_angle_x Delta angle X (radians) + * @param delta_angle_y Delta angle Y (radians) + * @param delta_angle_z Delta angle X (radians) + * @param delta_velocity_x Delta velocity X (m/s) + * @param delta_velocity_y Delta velocity Y (m/s) + * @param delta_velocity_z Delta velocity Z (m/s) + * @param joint_roll Joint ROLL (radians) + * @param joint_el Joint EL (radians) + * @param joint_az Joint AZ (radians) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float delta_time,float delta_angle_x,float delta_angle_y,float delta_angle_z,float delta_velocity_x,float delta_velocity_y,float delta_velocity_z,float joint_roll,float joint_el,float joint_az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_REPORT_LEN]; + _mav_put_float(buf, 0, delta_time); + _mav_put_float(buf, 4, delta_angle_x); + _mav_put_float(buf, 8, delta_angle_y); + _mav_put_float(buf, 12, delta_angle_z); + _mav_put_float(buf, 16, delta_velocity_x); + _mav_put_float(buf, 20, delta_velocity_y); + _mav_put_float(buf, 24, delta_velocity_z); + _mav_put_float(buf, 28, joint_roll); + _mav_put_float(buf, 32, joint_el); + _mav_put_float(buf, 36, joint_az); + _mav_put_uint8_t(buf, 40, target_system); + _mav_put_uint8_t(buf, 41, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#else + mavlink_gimbal_report_t packet; + packet.delta_time = delta_time; + packet.delta_angle_x = delta_angle_x; + packet.delta_angle_y = delta_angle_y; + packet.delta_angle_z = delta_angle_z; + packet.delta_velocity_x = delta_velocity_x; + packet.delta_velocity_y = delta_velocity_y; + packet.delta_velocity_z = delta_velocity_z; + packet.joint_roll = joint_roll; + packet.joint_el = joint_el; + packet.joint_az = joint_az; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +} + +/** + * @brief Encode a gimbal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_report_t* gimbal_report) +{ + return mavlink_msg_gimbal_report_pack(system_id, component_id, msg, gimbal_report->target_system, gimbal_report->target_component, gimbal_report->delta_time, gimbal_report->delta_angle_x, gimbal_report->delta_angle_y, gimbal_report->delta_angle_z, gimbal_report->delta_velocity_x, gimbal_report->delta_velocity_y, gimbal_report->delta_velocity_z, gimbal_report->joint_roll, gimbal_report->joint_el, gimbal_report->joint_az); +} + +/** + * @brief Encode a gimbal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_report_t* gimbal_report) +{ + return mavlink_msg_gimbal_report_pack_chan(system_id, component_id, chan, msg, gimbal_report->target_system, gimbal_report->target_component, gimbal_report->delta_time, gimbal_report->delta_angle_x, gimbal_report->delta_angle_y, gimbal_report->delta_angle_z, gimbal_report->delta_velocity_x, gimbal_report->delta_velocity_y, gimbal_report->delta_velocity_z, gimbal_report->joint_roll, gimbal_report->joint_el, gimbal_report->joint_az); +} + +/** + * @brief Send a gimbal_report message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param delta_time Time since last update (seconds) + * @param delta_angle_x Delta angle X (radians) + * @param delta_angle_y Delta angle Y (radians) + * @param delta_angle_z Delta angle X (radians) + * @param delta_velocity_x Delta velocity X (m/s) + * @param delta_velocity_y Delta velocity Y (m/s) + * @param delta_velocity_z Delta velocity Z (m/s) + * @param joint_roll Joint ROLL (radians) + * @param joint_el Joint EL (radians) + * @param joint_az Joint AZ (radians) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_report_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float delta_time, float delta_angle_x, float delta_angle_y, float delta_angle_z, float delta_velocity_x, float delta_velocity_y, float delta_velocity_z, float joint_roll, float joint_el, float joint_az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_REPORT_LEN]; + _mav_put_float(buf, 0, delta_time); + _mav_put_float(buf, 4, delta_angle_x); + _mav_put_float(buf, 8, delta_angle_y); + _mav_put_float(buf, 12, delta_angle_z); + _mav_put_float(buf, 16, delta_velocity_x); + _mav_put_float(buf, 20, delta_velocity_y); + _mav_put_float(buf, 24, delta_velocity_z); + _mav_put_float(buf, 28, joint_roll); + _mav_put_float(buf, 32, joint_el); + _mav_put_float(buf, 36, joint_az); + _mav_put_uint8_t(buf, 40, target_system); + _mav_put_uint8_t(buf, 41, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +#else + mavlink_gimbal_report_t packet; + packet.delta_time = delta_time; + packet.delta_angle_x = delta_angle_x; + packet.delta_angle_y = delta_angle_y; + packet.delta_angle_z = delta_angle_z; + packet.delta_velocity_x = delta_velocity_x; + packet.delta_velocity_y = delta_velocity_y; + packet.delta_velocity_z = delta_velocity_z; + packet.joint_roll = joint_roll; + packet.joint_el = joint_el; + packet.joint_az = joint_az; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float delta_time, float delta_angle_x, float delta_angle_y, float delta_angle_z, float delta_velocity_x, float delta_velocity_y, float delta_velocity_z, float joint_roll, float joint_el, float joint_az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, delta_time); + _mav_put_float(buf, 4, delta_angle_x); + _mav_put_float(buf, 8, delta_angle_y); + _mav_put_float(buf, 12, delta_angle_z); + _mav_put_float(buf, 16, delta_velocity_x); + _mav_put_float(buf, 20, delta_velocity_y); + _mav_put_float(buf, 24, delta_velocity_z); + _mav_put_float(buf, 28, joint_roll); + _mav_put_float(buf, 32, joint_el); + _mav_put_float(buf, 36, joint_az); + _mav_put_uint8_t(buf, 40, target_system); + _mav_put_uint8_t(buf, 41, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +#else + mavlink_gimbal_report_t *packet = (mavlink_gimbal_report_t *)msgbuf; + packet->delta_time = delta_time; + packet->delta_angle_x = delta_angle_x; + packet->delta_angle_y = delta_angle_y; + packet->delta_angle_z = delta_angle_z; + packet->delta_velocity_x = delta_velocity_x; + packet->delta_velocity_y = delta_velocity_y; + packet->delta_velocity_z = delta_velocity_z; + packet->joint_roll = joint_roll; + packet->joint_el = joint_el; + packet->joint_az = joint_az; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_REPORT UNPACKING + + +/** + * @brief Get field target_system from gimbal_report message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_report_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field target_component from gimbal_report message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_report_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field delta_time from gimbal_report message + * + * @return Time since last update (seconds) + */ +static inline float mavlink_msg_gimbal_report_get_delta_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field delta_angle_x from gimbal_report message + * + * @return Delta angle X (radians) + */ +static inline float mavlink_msg_gimbal_report_get_delta_angle_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field delta_angle_y from gimbal_report message + * + * @return Delta angle Y (radians) + */ +static inline float mavlink_msg_gimbal_report_get_delta_angle_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field delta_angle_z from gimbal_report message + * + * @return Delta angle X (radians) + */ +static inline float mavlink_msg_gimbal_report_get_delta_angle_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field delta_velocity_x from gimbal_report message + * + * @return Delta velocity X (m/s) + */ +static inline float mavlink_msg_gimbal_report_get_delta_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field delta_velocity_y from gimbal_report message + * + * @return Delta velocity Y (m/s) + */ +static inline float mavlink_msg_gimbal_report_get_delta_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field delta_velocity_z from gimbal_report message + * + * @return Delta velocity Z (m/s) + */ +static inline float mavlink_msg_gimbal_report_get_delta_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field joint_roll from gimbal_report message + * + * @return Joint ROLL (radians) + */ +static inline float mavlink_msg_gimbal_report_get_joint_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field joint_el from gimbal_report message + * + * @return Joint EL (radians) + */ +static inline float mavlink_msg_gimbal_report_get_joint_el(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field joint_az from gimbal_report message + * + * @return Joint AZ (radians) + */ +static inline float mavlink_msg_gimbal_report_get_joint_az(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a gimbal_report message into a struct + * + * @param msg The message to decode + * @param gimbal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_report_decode(const mavlink_message_t* msg, mavlink_gimbal_report_t* gimbal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + gimbal_report->delta_time = mavlink_msg_gimbal_report_get_delta_time(msg); + gimbal_report->delta_angle_x = mavlink_msg_gimbal_report_get_delta_angle_x(msg); + gimbal_report->delta_angle_y = mavlink_msg_gimbal_report_get_delta_angle_y(msg); + gimbal_report->delta_angle_z = mavlink_msg_gimbal_report_get_delta_angle_z(msg); + gimbal_report->delta_velocity_x = mavlink_msg_gimbal_report_get_delta_velocity_x(msg); + gimbal_report->delta_velocity_y = mavlink_msg_gimbal_report_get_delta_velocity_y(msg); + gimbal_report->delta_velocity_z = mavlink_msg_gimbal_report_get_delta_velocity_z(msg); + gimbal_report->joint_roll = mavlink_msg_gimbal_report_get_joint_roll(msg); + gimbal_report->joint_el = mavlink_msg_gimbal_report_get_joint_el(msg); + gimbal_report->joint_az = mavlink_msg_gimbal_report_get_joint_az(msg); + gimbal_report->target_system = mavlink_msg_gimbal_report_get_target_system(msg); + gimbal_report->target_component = mavlink_msg_gimbal_report_get_target_component(msg); +#else + memcpy(gimbal_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GIMBAL_REPORT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_torque_cmd_report.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_torque_cmd_report.h new file mode 100644 index 0000000..d6f52a5 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gimbal_torque_cmd_report.h @@ -0,0 +1,305 @@ +// MESSAGE GIMBAL_TORQUE_CMD_REPORT PACKING + +#define MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT 214 + +typedef struct __mavlink_gimbal_torque_cmd_report_t +{ + int16_t rl_torque_cmd; ///< Roll Torque Command + int16_t el_torque_cmd; ///< Elevation Torque Command + int16_t az_torque_cmd; ///< Azimuth Torque Command + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_gimbal_torque_cmd_report_t; + +#define MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN 8 +#define MAVLINK_MSG_ID_214_LEN 8 + +#define MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC 69 +#define MAVLINK_MSG_ID_214_CRC 69 + + + +#define MAVLINK_MESSAGE_INFO_GIMBAL_TORQUE_CMD_REPORT { \ + "GIMBAL_TORQUE_CMD_REPORT", \ + 5, \ + { { "rl_torque_cmd", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_gimbal_torque_cmd_report_t, rl_torque_cmd) }, \ + { "el_torque_cmd", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_gimbal_torque_cmd_report_t, el_torque_cmd) }, \ + { "az_torque_cmd", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_gimbal_torque_cmd_report_t, az_torque_cmd) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_gimbal_torque_cmd_report_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_gimbal_torque_cmd_report_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a gimbal_torque_cmd_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param rl_torque_cmd Roll Torque Command + * @param el_torque_cmd Elevation Torque Command + * @param az_torque_cmd Azimuth Torque Command + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_torque_cmd_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t rl_torque_cmd, int16_t el_torque_cmd, int16_t az_torque_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN]; + _mav_put_int16_t(buf, 0, rl_torque_cmd); + _mav_put_int16_t(buf, 2, el_torque_cmd); + _mav_put_int16_t(buf, 4, az_torque_cmd); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#else + mavlink_gimbal_torque_cmd_report_t packet; + packet.rl_torque_cmd = rl_torque_cmd; + packet.el_torque_cmd = el_torque_cmd; + packet.az_torque_cmd = az_torque_cmd; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +} + +/** + * @brief Pack a gimbal_torque_cmd_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param rl_torque_cmd Roll Torque Command + * @param el_torque_cmd Elevation Torque Command + * @param az_torque_cmd Azimuth Torque Command + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_torque_cmd_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t rl_torque_cmd,int16_t el_torque_cmd,int16_t az_torque_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN]; + _mav_put_int16_t(buf, 0, rl_torque_cmd); + _mav_put_int16_t(buf, 2, el_torque_cmd); + _mav_put_int16_t(buf, 4, az_torque_cmd); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#else + mavlink_gimbal_torque_cmd_report_t packet; + packet.rl_torque_cmd = rl_torque_cmd; + packet.el_torque_cmd = el_torque_cmd; + packet.az_torque_cmd = az_torque_cmd; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +} + +/** + * @brief Encode a gimbal_torque_cmd_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_torque_cmd_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_torque_cmd_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_torque_cmd_report_t* gimbal_torque_cmd_report) +{ + return mavlink_msg_gimbal_torque_cmd_report_pack(system_id, component_id, msg, gimbal_torque_cmd_report->target_system, gimbal_torque_cmd_report->target_component, gimbal_torque_cmd_report->rl_torque_cmd, gimbal_torque_cmd_report->el_torque_cmd, gimbal_torque_cmd_report->az_torque_cmd); +} + +/** + * @brief Encode a gimbal_torque_cmd_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_torque_cmd_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_torque_cmd_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_torque_cmd_report_t* gimbal_torque_cmd_report) +{ + return mavlink_msg_gimbal_torque_cmd_report_pack_chan(system_id, component_id, chan, msg, gimbal_torque_cmd_report->target_system, gimbal_torque_cmd_report->target_component, gimbal_torque_cmd_report->rl_torque_cmd, gimbal_torque_cmd_report->el_torque_cmd, gimbal_torque_cmd_report->az_torque_cmd); +} + +/** + * @brief Send a gimbal_torque_cmd_report message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param rl_torque_cmd Roll Torque Command + * @param el_torque_cmd Elevation Torque Command + * @param az_torque_cmd Azimuth Torque Command + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_torque_cmd_report_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t rl_torque_cmd, int16_t el_torque_cmd, int16_t az_torque_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN]; + _mav_put_int16_t(buf, 0, rl_torque_cmd); + _mav_put_int16_t(buf, 2, el_torque_cmd); + _mav_put_int16_t(buf, 4, az_torque_cmd); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +#else + mavlink_gimbal_torque_cmd_report_t packet; + packet.rl_torque_cmd = rl_torque_cmd; + packet.el_torque_cmd = el_torque_cmd; + packet.az_torque_cmd = az_torque_cmd; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_torque_cmd_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t rl_torque_cmd, int16_t el_torque_cmd, int16_t az_torque_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, rl_torque_cmd); + _mav_put_int16_t(buf, 2, el_torque_cmd); + _mav_put_int16_t(buf, 4, az_torque_cmd); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, buf, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +#else + mavlink_gimbal_torque_cmd_report_t *packet = (mavlink_gimbal_torque_cmd_report_t *)msgbuf; + packet->rl_torque_cmd = rl_torque_cmd; + packet->el_torque_cmd = el_torque_cmd; + packet->az_torque_cmd = az_torque_cmd; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_TORQUE_CMD_REPORT UNPACKING + + +/** + * @brief Get field target_system from gimbal_torque_cmd_report message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_torque_cmd_report_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field target_component from gimbal_torque_cmd_report message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_torque_cmd_report_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field rl_torque_cmd from gimbal_torque_cmd_report message + * + * @return Roll Torque Command + */ +static inline int16_t mavlink_msg_gimbal_torque_cmd_report_get_rl_torque_cmd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field el_torque_cmd from gimbal_torque_cmd_report message + * + * @return Elevation Torque Command + */ +static inline int16_t mavlink_msg_gimbal_torque_cmd_report_get_el_torque_cmd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Get field az_torque_cmd from gimbal_torque_cmd_report message + * + * @return Azimuth Torque Command + */ +static inline int16_t mavlink_msg_gimbal_torque_cmd_report_get_az_torque_cmd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Decode a gimbal_torque_cmd_report message into a struct + * + * @param msg The message to decode + * @param gimbal_torque_cmd_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_torque_cmd_report_decode(const mavlink_message_t* msg, mavlink_gimbal_torque_cmd_report_t* gimbal_torque_cmd_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + gimbal_torque_cmd_report->rl_torque_cmd = mavlink_msg_gimbal_torque_cmd_report_get_rl_torque_cmd(msg); + gimbal_torque_cmd_report->el_torque_cmd = mavlink_msg_gimbal_torque_cmd_report_get_el_torque_cmd(msg); + gimbal_torque_cmd_report->az_torque_cmd = mavlink_msg_gimbal_torque_cmd_report_get_az_torque_cmd(msg); + gimbal_torque_cmd_report->target_system = mavlink_msg_gimbal_torque_cmd_report_get_target_system(msg); + gimbal_torque_cmd_report->target_component = mavlink_msg_gimbal_torque_cmd_report_get_target_component(msg); +#else + memcpy(gimbal_torque_cmd_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_request.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_request.h new file mode 100644 index 0000000..5d747e4 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_request.h @@ -0,0 +1,257 @@ +// MESSAGE GOPRO_GET_REQUEST PACKING + +#define MAVLINK_MSG_ID_GOPRO_GET_REQUEST 216 + +typedef struct __mavlink_gopro_get_request_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t cmd_id; ///< Command ID +} mavlink_gopro_get_request_t; + +#define MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN 3 +#define MAVLINK_MSG_ID_216_LEN 3 + +#define MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC 50 +#define MAVLINK_MSG_ID_216_CRC 50 + + + +#define MAVLINK_MESSAGE_INFO_GOPRO_GET_REQUEST { \ + "GOPRO_GET_REQUEST", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gopro_get_request_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gopro_get_request_t, target_component) }, \ + { "cmd_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gopro_get_request_t, cmd_id) }, \ + } \ +} + + +/** + * @brief Pack a gopro_get_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_get_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t cmd_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#else + mavlink_gopro_get_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_GET_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a gopro_get_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_get_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t cmd_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#else + mavlink_gopro_get_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_GET_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a gopro_get_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gopro_get_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_get_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gopro_get_request_t* gopro_get_request) +{ + return mavlink_msg_gopro_get_request_pack(system_id, component_id, msg, gopro_get_request->target_system, gopro_get_request->target_component, gopro_get_request->cmd_id); +} + +/** + * @brief Encode a gopro_get_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gopro_get_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_get_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gopro_get_request_t* gopro_get_request) +{ + return mavlink_msg_gopro_get_request_pack_chan(system_id, component_id, chan, msg, gopro_get_request->target_system, gopro_get_request->target_component, gopro_get_request->cmd_id); +} + +/** + * @brief Send a gopro_get_request message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gopro_get_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t cmd_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +#else + mavlink_gopro_get_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gopro_get_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t cmd_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +#else + mavlink_gopro_get_request_t *packet = (mavlink_gopro_get_request_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->cmd_id = cmd_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, (const char *)packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_REQUEST, (const char *)packet, MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GOPRO_GET_REQUEST UNPACKING + + +/** + * @brief Get field target_system from gopro_get_request message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gopro_get_request_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from gopro_get_request message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gopro_get_request_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field cmd_id from gopro_get_request message + * + * @return Command ID + */ +static inline uint8_t mavlink_msg_gopro_get_request_get_cmd_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a gopro_get_request message into a struct + * + * @param msg The message to decode + * @param gopro_get_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_gopro_get_request_decode(const mavlink_message_t* msg, mavlink_gopro_get_request_t* gopro_get_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + gopro_get_request->target_system = mavlink_msg_gopro_get_request_get_target_system(msg); + gopro_get_request->target_component = mavlink_msg_gopro_get_request_get_target_component(msg); + gopro_get_request->cmd_id = mavlink_msg_gopro_get_request_get_cmd_id(msg); +#else + memcpy(gopro_get_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GOPRO_GET_REQUEST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_response.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_response.h new file mode 100644 index 0000000..bacd9da --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_get_response.h @@ -0,0 +1,249 @@ +// MESSAGE GOPRO_GET_RESPONSE PACKING + +#define MAVLINK_MSG_ID_GOPRO_GET_RESPONSE 217 + +typedef struct __mavlink_gopro_get_response_t +{ + uint8_t cmd_id; ///< Command ID + uint8_t status; ///< Status + uint8_t value[4]; ///< Value +} mavlink_gopro_get_response_t; + +#define MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN 6 +#define MAVLINK_MSG_ID_217_LEN 6 + +#define MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC 202 +#define MAVLINK_MSG_ID_217_CRC 202 + +#define MAVLINK_MSG_GOPRO_GET_RESPONSE_FIELD_VALUE_LEN 4 + +#define MAVLINK_MESSAGE_INFO_GOPRO_GET_RESPONSE { \ + "GOPRO_GET_RESPONSE", \ + 3, \ + { { "cmd_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gopro_get_response_t, cmd_id) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gopro_get_response_t, status) }, \ + { "value", NULL, MAVLINK_TYPE_UINT8_T, 4, 2, offsetof(mavlink_gopro_get_response_t, value) }, \ + } \ +} + + +/** + * @brief Pack a gopro_get_response message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param cmd_id Command ID + * @param status Status + * @param value Value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_get_response_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t cmd_id, uint8_t status, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + _mav_put_uint8_t_array(buf, 2, value, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#else + mavlink_gopro_get_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_GET_RESPONSE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +} + +/** + * @brief Pack a gopro_get_response message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_id Command ID + * @param status Status + * @param value Value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_get_response_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t cmd_id,uint8_t status,const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + _mav_put_uint8_t_array(buf, 2, value, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#else + mavlink_gopro_get_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_GET_RESPONSE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +} + +/** + * @brief Encode a gopro_get_response struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gopro_get_response C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_get_response_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gopro_get_response_t* gopro_get_response) +{ + return mavlink_msg_gopro_get_response_pack(system_id, component_id, msg, gopro_get_response->cmd_id, gopro_get_response->status, gopro_get_response->value); +} + +/** + * @brief Encode a gopro_get_response struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gopro_get_response C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_get_response_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gopro_get_response_t* gopro_get_response) +{ + return mavlink_msg_gopro_get_response_pack_chan(system_id, component_id, chan, msg, gopro_get_response->cmd_id, gopro_get_response->status, gopro_get_response->value); +} + +/** + * @brief Send a gopro_get_response message + * @param chan MAVLink channel to send the message + * + * @param cmd_id Command ID + * @param status Status + * @param value Value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gopro_get_response_send(mavlink_channel_t chan, uint8_t cmd_id, uint8_t status, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + _mav_put_uint8_t_array(buf, 2, value, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +#else + mavlink_gopro_get_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gopro_get_response_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cmd_id, uint8_t status, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + _mav_put_uint8_t_array(buf, 2, value, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +#else + mavlink_gopro_get_response_t *packet = (mavlink_gopro_get_response_t *)msgbuf; + packet->cmd_id = cmd_id; + packet->status = status; + mav_array_memcpy(packet->value, value, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, (const char *)packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE, (const char *)packet, MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GOPRO_GET_RESPONSE UNPACKING + + +/** + * @brief Get field cmd_id from gopro_get_response message + * + * @return Command ID + */ +static inline uint8_t mavlink_msg_gopro_get_response_get_cmd_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field status from gopro_get_response message + * + * @return Status + */ +static inline uint8_t mavlink_msg_gopro_get_response_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field value from gopro_get_response message + * + * @return Value + */ +static inline uint16_t mavlink_msg_gopro_get_response_get_value(const mavlink_message_t* msg, uint8_t *value) +{ + return _MAV_RETURN_uint8_t_array(msg, value, 4, 2); +} + +/** + * @brief Decode a gopro_get_response message into a struct + * + * @param msg The message to decode + * @param gopro_get_response C-struct to decode the message contents into + */ +static inline void mavlink_msg_gopro_get_response_decode(const mavlink_message_t* msg, mavlink_gopro_get_response_t* gopro_get_response) +{ +#if MAVLINK_NEED_BYTE_SWAP + gopro_get_response->cmd_id = mavlink_msg_gopro_get_response_get_cmd_id(msg); + gopro_get_response->status = mavlink_msg_gopro_get_response_get_status(msg); + mavlink_msg_gopro_get_response_get_value(msg, gopro_get_response->value); +#else + memcpy(gopro_get_response, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GOPRO_GET_RESPONSE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_heartbeat.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_heartbeat.h new file mode 100644 index 0000000..b41e9a2 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_heartbeat.h @@ -0,0 +1,257 @@ +// MESSAGE GOPRO_HEARTBEAT PACKING + +#define MAVLINK_MSG_ID_GOPRO_HEARTBEAT 215 + +typedef struct __mavlink_gopro_heartbeat_t +{ + uint8_t status; ///< Status + uint8_t capture_mode; ///< Current capture mode + uint8_t flags; ///< additional status bits +} mavlink_gopro_heartbeat_t; + +#define MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_ID_215_LEN 3 + +#define MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC 101 +#define MAVLINK_MSG_ID_215_CRC 101 + + + +#define MAVLINK_MESSAGE_INFO_GOPRO_HEARTBEAT { \ + "GOPRO_HEARTBEAT", \ + 3, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gopro_heartbeat_t, status) }, \ + { "capture_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gopro_heartbeat_t, capture_mode) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gopro_heartbeat_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a gopro_heartbeat message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Status + * @param capture_mode Current capture mode + * @param flags additional status bits + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t status, uint8_t capture_mode, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_uint8_t(buf, 1, capture_mode); + _mav_put_uint8_t(buf, 2, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#else + mavlink_gopro_heartbeat_t packet; + packet.status = status; + packet.capture_mode = capture_mode; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_HEARTBEAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +} + +/** + * @brief Pack a gopro_heartbeat message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Status + * @param capture_mode Current capture mode + * @param flags additional status bits + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t status,uint8_t capture_mode,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_uint8_t(buf, 1, capture_mode); + _mav_put_uint8_t(buf, 2, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#else + mavlink_gopro_heartbeat_t packet; + packet.status = status; + packet.capture_mode = capture_mode; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_HEARTBEAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +} + +/** + * @brief Encode a gopro_heartbeat struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gopro_heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gopro_heartbeat_t* gopro_heartbeat) +{ + return mavlink_msg_gopro_heartbeat_pack(system_id, component_id, msg, gopro_heartbeat->status, gopro_heartbeat->capture_mode, gopro_heartbeat->flags); +} + +/** + * @brief Encode a gopro_heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gopro_heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gopro_heartbeat_t* gopro_heartbeat) +{ + return mavlink_msg_gopro_heartbeat_pack_chan(system_id, component_id, chan, msg, gopro_heartbeat->status, gopro_heartbeat->capture_mode, gopro_heartbeat->flags); +} + +/** + * @brief Send a gopro_heartbeat message + * @param chan MAVLink channel to send the message + * + * @param status Status + * @param capture_mode Current capture mode + * @param flags additional status bits + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gopro_heartbeat_send(mavlink_channel_t chan, uint8_t status, uint8_t capture_mode, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_uint8_t(buf, 1, capture_mode); + _mav_put_uint8_t(buf, 2, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +#else + mavlink_gopro_heartbeat_t packet; + packet.status = status; + packet.capture_mode = capture_mode; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gopro_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t status, uint8_t capture_mode, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, status); + _mav_put_uint8_t(buf, 1, capture_mode); + _mav_put_uint8_t(buf, 2, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, buf, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +#else + mavlink_gopro_heartbeat_t *packet = (mavlink_gopro_heartbeat_t *)msgbuf; + packet->status = status; + packet->capture_mode = capture_mode; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GOPRO_HEARTBEAT UNPACKING + + +/** + * @brief Get field status from gopro_heartbeat message + * + * @return Status + */ +static inline uint8_t mavlink_msg_gopro_heartbeat_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field capture_mode from gopro_heartbeat message + * + * @return Current capture mode + */ +static inline uint8_t mavlink_msg_gopro_heartbeat_get_capture_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field flags from gopro_heartbeat message + * + * @return additional status bits + */ +static inline uint8_t mavlink_msg_gopro_heartbeat_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a gopro_heartbeat message into a struct + * + * @param msg The message to decode + * @param gopro_heartbeat C-struct to decode the message contents into + */ +static inline void mavlink_msg_gopro_heartbeat_decode(const mavlink_message_t* msg, mavlink_gopro_heartbeat_t* gopro_heartbeat) +{ +#if MAVLINK_NEED_BYTE_SWAP + gopro_heartbeat->status = mavlink_msg_gopro_heartbeat_get_status(msg); + gopro_heartbeat->capture_mode = mavlink_msg_gopro_heartbeat_get_capture_mode(msg); + gopro_heartbeat->flags = mavlink_msg_gopro_heartbeat_get_flags(msg); +#else + memcpy(gopro_heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GOPRO_HEARTBEAT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_request.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_request.h new file mode 100644 index 0000000..f066c8e --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_request.h @@ -0,0 +1,273 @@ +// MESSAGE GOPRO_SET_REQUEST PACKING + +#define MAVLINK_MSG_ID_GOPRO_SET_REQUEST 218 + +typedef struct __mavlink_gopro_set_request_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t cmd_id; ///< Command ID + uint8_t value[4]; ///< Value +} mavlink_gopro_set_request_t; + +#define MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN 7 +#define MAVLINK_MSG_ID_218_LEN 7 + +#define MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC 17 +#define MAVLINK_MSG_ID_218_CRC 17 + +#define MAVLINK_MSG_GOPRO_SET_REQUEST_FIELD_VALUE_LEN 4 + +#define MAVLINK_MESSAGE_INFO_GOPRO_SET_REQUEST { \ + "GOPRO_SET_REQUEST", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gopro_set_request_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gopro_set_request_t, target_component) }, \ + { "cmd_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gopro_set_request_t, cmd_id) }, \ + { "value", NULL, MAVLINK_TYPE_UINT8_T, 4, 3, offsetof(mavlink_gopro_set_request_t, value) }, \ + } \ +} + + +/** + * @brief Pack a gopro_set_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + * @param value Value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_set_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t cmd_id, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + _mav_put_uint8_t_array(buf, 3, value, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#else + mavlink_gopro_set_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_SET_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a gopro_set_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + * @param value Value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_set_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t cmd_id,const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + _mav_put_uint8_t_array(buf, 3, value, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#else + mavlink_gopro_set_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_SET_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a gopro_set_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gopro_set_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_set_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gopro_set_request_t* gopro_set_request) +{ + return mavlink_msg_gopro_set_request_pack(system_id, component_id, msg, gopro_set_request->target_system, gopro_set_request->target_component, gopro_set_request->cmd_id, gopro_set_request->value); +} + +/** + * @brief Encode a gopro_set_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gopro_set_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_set_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gopro_set_request_t* gopro_set_request) +{ + return mavlink_msg_gopro_set_request_pack_chan(system_id, component_id, chan, msg, gopro_set_request->target_system, gopro_set_request->target_component, gopro_set_request->cmd_id, gopro_set_request->value); +} + +/** + * @brief Send a gopro_set_request message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param cmd_id Command ID + * @param value Value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gopro_set_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t cmd_id, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + _mav_put_uint8_t_array(buf, 3, value, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +#else + mavlink_gopro_set_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.cmd_id = cmd_id; + mav_array_memcpy(packet.value, value, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gopro_set_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t cmd_id, const uint8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, cmd_id); + _mav_put_uint8_t_array(buf, 3, value, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, buf, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +#else + mavlink_gopro_set_request_t *packet = (mavlink_gopro_set_request_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->cmd_id = cmd_id; + mav_array_memcpy(packet->value, value, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, (const char *)packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_REQUEST, (const char *)packet, MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GOPRO_SET_REQUEST UNPACKING + + +/** + * @brief Get field target_system from gopro_set_request message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gopro_set_request_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from gopro_set_request message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gopro_set_request_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field cmd_id from gopro_set_request message + * + * @return Command ID + */ +static inline uint8_t mavlink_msg_gopro_set_request_get_cmd_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field value from gopro_set_request message + * + * @return Value + */ +static inline uint16_t mavlink_msg_gopro_set_request_get_value(const mavlink_message_t* msg, uint8_t *value) +{ + return _MAV_RETURN_uint8_t_array(msg, value, 4, 3); +} + +/** + * @brief Decode a gopro_set_request message into a struct + * + * @param msg The message to decode + * @param gopro_set_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_gopro_set_request_decode(const mavlink_message_t* msg, mavlink_gopro_set_request_t* gopro_set_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + gopro_set_request->target_system = mavlink_msg_gopro_set_request_get_target_system(msg); + gopro_set_request->target_component = mavlink_msg_gopro_set_request_get_target_component(msg); + gopro_set_request->cmd_id = mavlink_msg_gopro_set_request_get_cmd_id(msg); + mavlink_msg_gopro_set_request_get_value(msg, gopro_set_request->value); +#else + memcpy(gopro_set_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GOPRO_SET_REQUEST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_response.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_response.h new file mode 100644 index 0000000..d6755a3 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gopro_set_response.h @@ -0,0 +1,233 @@ +// MESSAGE GOPRO_SET_RESPONSE PACKING + +#define MAVLINK_MSG_ID_GOPRO_SET_RESPONSE 219 + +typedef struct __mavlink_gopro_set_response_t +{ + uint8_t cmd_id; ///< Command ID + uint8_t status; ///< Status +} mavlink_gopro_set_response_t; + +#define MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN 2 +#define MAVLINK_MSG_ID_219_LEN 2 + +#define MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC 162 +#define MAVLINK_MSG_ID_219_CRC 162 + + + +#define MAVLINK_MESSAGE_INFO_GOPRO_SET_RESPONSE { \ + "GOPRO_SET_RESPONSE", \ + 2, \ + { { "cmd_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gopro_set_response_t, cmd_id) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gopro_set_response_t, status) }, \ + } \ +} + + +/** + * @brief Pack a gopro_set_response message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param cmd_id Command ID + * @param status Status + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_set_response_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t cmd_id, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#else + mavlink_gopro_set_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_SET_RESPONSE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +} + +/** + * @brief Pack a gopro_set_response message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_id Command ID + * @param status Status + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gopro_set_response_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t cmd_id,uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#else + mavlink_gopro_set_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GOPRO_SET_RESPONSE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +} + +/** + * @brief Encode a gopro_set_response struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gopro_set_response C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_set_response_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gopro_set_response_t* gopro_set_response) +{ + return mavlink_msg_gopro_set_response_pack(system_id, component_id, msg, gopro_set_response->cmd_id, gopro_set_response->status); +} + +/** + * @brief Encode a gopro_set_response struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gopro_set_response C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gopro_set_response_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gopro_set_response_t* gopro_set_response) +{ + return mavlink_msg_gopro_set_response_pack_chan(system_id, component_id, chan, msg, gopro_set_response->cmd_id, gopro_set_response->status); +} + +/** + * @brief Send a gopro_set_response message + * @param chan MAVLink channel to send the message + * + * @param cmd_id Command ID + * @param status Status + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gopro_set_response_send(mavlink_channel_t chan, uint8_t cmd_id, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN]; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +#else + mavlink_gopro_set_response_t packet; + packet.cmd_id = cmd_id; + packet.status = status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, (const char *)&packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gopro_set_response_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cmd_id, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, cmd_id); + _mav_put_uint8_t(buf, 1, status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, buf, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +#else + mavlink_gopro_set_response_t *packet = (mavlink_gopro_set_response_t *)msgbuf; + packet->cmd_id = cmd_id; + packet->status = status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, (const char *)packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE, (const char *)packet, MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GOPRO_SET_RESPONSE UNPACKING + + +/** + * @brief Get field cmd_id from gopro_set_response message + * + * @return Command ID + */ +static inline uint8_t mavlink_msg_gopro_set_response_get_cmd_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field status from gopro_set_response message + * + * @return Status + */ +static inline uint8_t mavlink_msg_gopro_set_response_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a gopro_set_response message into a struct + * + * @param msg The message to decode + * @param gopro_set_response C-struct to decode the message contents into + */ +static inline void mavlink_msg_gopro_set_response_decode(const mavlink_message_t* msg, mavlink_gopro_set_response_t* gopro_set_response) +{ +#if MAVLINK_NEED_BYTE_SWAP + gopro_set_response->cmd_id = mavlink_msg_gopro_set_response_get_cmd_id(msg); + gopro_set_response->status = mavlink_msg_gopro_set_response_get_status(msg); +#else + memcpy(gopro_set_response, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GOPRO_SET_RESPONSE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gps_accuracy.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gps_accuracy.h new file mode 100644 index 0000000..86479fb --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_gps_accuracy.h @@ -0,0 +1,353 @@ +// MESSAGE GPS_ACCURACY PACKING + +#define MAVLINK_MSG_ID_GPS_ACCURACY 225 + +typedef struct __mavlink_gps_accuracy_t +{ + float h_acc; ///< GPS-reported horizontal accuracy + float s_acc; ///< GPS-reported speed accuracy + float h_vel_filt; ///< GPS-reported, filtered horizontal velocity + float v_vel_filt; ///< GPS-reported, filtered vertical velocity + float p_drift; ///< GPS position drift + uint8_t instance; ///< Which instance of GPS we're reporting on + uint8_t ekf_check_mask; ///< Which fields pass EKF checks +} mavlink_gps_accuracy_t; + +#define MAVLINK_MSG_ID_GPS_ACCURACY_LEN 22 +#define MAVLINK_MSG_ID_225_LEN 22 + +#define MAVLINK_MSG_ID_GPS_ACCURACY_CRC 76 +#define MAVLINK_MSG_ID_225_CRC 76 + + + +#define MAVLINK_MESSAGE_INFO_GPS_ACCURACY { \ + "GPS_ACCURACY", \ + 7, \ + { { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_gps_accuracy_t, h_acc) }, \ + { "s_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gps_accuracy_t, s_acc) }, \ + { "h_vel_filt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gps_accuracy_t, h_vel_filt) }, \ + { "v_vel_filt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gps_accuracy_t, v_vel_filt) }, \ + { "p_drift", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gps_accuracy_t, p_drift) }, \ + { "instance", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gps_accuracy_t, instance) }, \ + { "ekf_check_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gps_accuracy_t, ekf_check_mask) }, \ + } \ +} + + +/** + * @brief Pack a gps_accuracy message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param instance Which instance of GPS we're reporting on + * @param h_acc GPS-reported horizontal accuracy + * @param s_acc GPS-reported speed accuracy + * @param h_vel_filt GPS-reported, filtered horizontal velocity + * @param v_vel_filt GPS-reported, filtered vertical velocity + * @param p_drift GPS position drift + * @param ekf_check_mask Which fields pass EKF checks + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_accuracy_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t instance, float h_acc, float s_acc, float h_vel_filt, float v_vel_filt, float p_drift, uint8_t ekf_check_mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_ACCURACY_LEN]; + _mav_put_float(buf, 0, h_acc); + _mav_put_float(buf, 4, s_acc); + _mav_put_float(buf, 8, h_vel_filt); + _mav_put_float(buf, 12, v_vel_filt); + _mav_put_float(buf, 16, p_drift); + _mav_put_uint8_t(buf, 20, instance); + _mav_put_uint8_t(buf, 21, ekf_check_mask); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#else + mavlink_gps_accuracy_t packet; + packet.h_acc = h_acc; + packet.s_acc = s_acc; + packet.h_vel_filt = h_vel_filt; + packet.v_vel_filt = v_vel_filt; + packet.p_drift = p_drift; + packet.instance = instance; + packet.ekf_check_mask = ekf_check_mask; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_ACCURACY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +} + +/** + * @brief Pack a gps_accuracy message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param instance Which instance of GPS we're reporting on + * @param h_acc GPS-reported horizontal accuracy + * @param s_acc GPS-reported speed accuracy + * @param h_vel_filt GPS-reported, filtered horizontal velocity + * @param v_vel_filt GPS-reported, filtered vertical velocity + * @param p_drift GPS position drift + * @param ekf_check_mask Which fields pass EKF checks + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_accuracy_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t instance,float h_acc,float s_acc,float h_vel_filt,float v_vel_filt,float p_drift,uint8_t ekf_check_mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_ACCURACY_LEN]; + _mav_put_float(buf, 0, h_acc); + _mav_put_float(buf, 4, s_acc); + _mav_put_float(buf, 8, h_vel_filt); + _mav_put_float(buf, 12, v_vel_filt); + _mav_put_float(buf, 16, p_drift); + _mav_put_uint8_t(buf, 20, instance); + _mav_put_uint8_t(buf, 21, ekf_check_mask); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#else + mavlink_gps_accuracy_t packet; + packet.h_acc = h_acc; + packet.s_acc = s_acc; + packet.h_vel_filt = h_vel_filt; + packet.v_vel_filt = v_vel_filt; + packet.p_drift = p_drift; + packet.instance = instance; + packet.ekf_check_mask = ekf_check_mask; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_ACCURACY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +} + +/** + * @brief Encode a gps_accuracy struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_accuracy C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_accuracy_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_accuracy_t* gps_accuracy) +{ + return mavlink_msg_gps_accuracy_pack(system_id, component_id, msg, gps_accuracy->instance, gps_accuracy->h_acc, gps_accuracy->s_acc, gps_accuracy->h_vel_filt, gps_accuracy->v_vel_filt, gps_accuracy->p_drift, gps_accuracy->ekf_check_mask); +} + +/** + * @brief Encode a gps_accuracy struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_accuracy C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_accuracy_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_accuracy_t* gps_accuracy) +{ + return mavlink_msg_gps_accuracy_pack_chan(system_id, component_id, chan, msg, gps_accuracy->instance, gps_accuracy->h_acc, gps_accuracy->s_acc, gps_accuracy->h_vel_filt, gps_accuracy->v_vel_filt, gps_accuracy->p_drift, gps_accuracy->ekf_check_mask); +} + +/** + * @brief Send a gps_accuracy message + * @param chan MAVLink channel to send the message + * + * @param instance Which instance of GPS we're reporting on + * @param h_acc GPS-reported horizontal accuracy + * @param s_acc GPS-reported speed accuracy + * @param h_vel_filt GPS-reported, filtered horizontal velocity + * @param v_vel_filt GPS-reported, filtered vertical velocity + * @param p_drift GPS position drift + * @param ekf_check_mask Which fields pass EKF checks + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_accuracy_send(mavlink_channel_t chan, uint8_t instance, float h_acc, float s_acc, float h_vel_filt, float v_vel_filt, float p_drift, uint8_t ekf_check_mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_ACCURACY_LEN]; + _mav_put_float(buf, 0, h_acc); + _mav_put_float(buf, 4, s_acc); + _mav_put_float(buf, 8, h_vel_filt); + _mav_put_float(buf, 12, v_vel_filt); + _mav_put_float(buf, 16, p_drift); + _mav_put_uint8_t(buf, 20, instance); + _mav_put_uint8_t(buf, 21, ekf_check_mask); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +#else + mavlink_gps_accuracy_t packet; + packet.h_acc = h_acc; + packet.s_acc = s_acc; + packet.h_vel_filt = h_vel_filt; + packet.v_vel_filt = v_vel_filt; + packet.p_drift = p_drift; + packet.instance = instance; + packet.ekf_check_mask = ekf_check_mask; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, (const char *)&packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, (const char *)&packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_ACCURACY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_accuracy_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t instance, float h_acc, float s_acc, float h_vel_filt, float v_vel_filt, float p_drift, uint8_t ekf_check_mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, h_acc); + _mav_put_float(buf, 4, s_acc); + _mav_put_float(buf, 8, h_vel_filt); + _mav_put_float(buf, 12, v_vel_filt); + _mav_put_float(buf, 16, p_drift); + _mav_put_uint8_t(buf, 20, instance); + _mav_put_uint8_t(buf, 21, ekf_check_mask); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, buf, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +#else + mavlink_gps_accuracy_t *packet = (mavlink_gps_accuracy_t *)msgbuf; + packet->h_acc = h_acc; + packet->s_acc = s_acc; + packet->h_vel_filt = h_vel_filt; + packet->v_vel_filt = v_vel_filt; + packet->p_drift = p_drift; + packet->instance = instance; + packet->ekf_check_mask = ekf_check_mask; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, (const char *)packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN, MAVLINK_MSG_ID_GPS_ACCURACY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_ACCURACY, (const char *)packet, MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_ACCURACY UNPACKING + + +/** + * @brief Get field instance from gps_accuracy message + * + * @return Which instance of GPS we're reporting on + */ +static inline uint8_t mavlink_msg_gps_accuracy_get_instance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field h_acc from gps_accuracy message + * + * @return GPS-reported horizontal accuracy + */ +static inline float mavlink_msg_gps_accuracy_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field s_acc from gps_accuracy message + * + * @return GPS-reported speed accuracy + */ +static inline float mavlink_msg_gps_accuracy_get_s_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field h_vel_filt from gps_accuracy message + * + * @return GPS-reported, filtered horizontal velocity + */ +static inline float mavlink_msg_gps_accuracy_get_h_vel_filt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field v_vel_filt from gps_accuracy message + * + * @return GPS-reported, filtered vertical velocity + */ +static inline float mavlink_msg_gps_accuracy_get_v_vel_filt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field p_drift from gps_accuracy message + * + * @return GPS position drift + */ +static inline float mavlink_msg_gps_accuracy_get_p_drift(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field ekf_check_mask from gps_accuracy message + * + * @return Which fields pass EKF checks + */ +static inline uint8_t mavlink_msg_gps_accuracy_get_ekf_check_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a gps_accuracy message into a struct + * + * @param msg The message to decode + * @param gps_accuracy C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_accuracy_decode(const mavlink_message_t* msg, mavlink_gps_accuracy_t* gps_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_accuracy->h_acc = mavlink_msg_gps_accuracy_get_h_acc(msg); + gps_accuracy->s_acc = mavlink_msg_gps_accuracy_get_s_acc(msg); + gps_accuracy->h_vel_filt = mavlink_msg_gps_accuracy_get_h_vel_filt(msg); + gps_accuracy->v_vel_filt = mavlink_msg_gps_accuracy_get_v_vel_filt(msg); + gps_accuracy->p_drift = mavlink_msg_gps_accuracy_get_p_drift(msg); + gps_accuracy->instance = mavlink_msg_gps_accuracy_get_instance(msg); + gps_accuracy->ekf_check_mask = mavlink_msg_gps_accuracy_get_ekf_check_mask(msg); +#else + memcpy(gps_accuracy, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_ACCURACY_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_hwstatus.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_hwstatus.h new file mode 100644 index 0000000..acf031f --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_hwstatus.h @@ -0,0 +1,233 @@ +// MESSAGE HWSTATUS PACKING + +#define MAVLINK_MSG_ID_HWSTATUS 165 + +typedef struct __mavlink_hwstatus_t +{ + uint16_t Vcc; ///< board voltage (mV) + uint8_t I2Cerr; ///< I2C error count +} mavlink_hwstatus_t; + +#define MAVLINK_MSG_ID_HWSTATUS_LEN 3 +#define MAVLINK_MSG_ID_165_LEN 3 + +#define MAVLINK_MSG_ID_HWSTATUS_CRC 21 +#define MAVLINK_MSG_ID_165_CRC 21 + + + +#define MAVLINK_MESSAGE_INFO_HWSTATUS { \ + "HWSTATUS", \ + 2, \ + { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \ + { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \ + } \ +} + + +/** + * @brief Pack a hwstatus message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param Vcc board voltage (mV) + * @param I2Cerr I2C error count + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Vcc, uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#else + mavlink_hwstatus_t packet; + packet.Vcc = Vcc; + packet.I2Cerr = I2Cerr; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HWSTATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +} + +/** + * @brief Pack a hwstatus message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param Vcc board voltage (mV) + * @param I2Cerr I2C error count + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Vcc,uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#else + mavlink_hwstatus_t packet; + packet.Vcc = Vcc; + packet.I2Cerr = I2Cerr; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HWSTATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +} + +/** + * @brief Encode a hwstatus struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hwstatus C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) +{ + return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr); +} + +/** + * @brief Encode a hwstatus struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hwstatus C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) +{ + return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); +} + +/** + * @brief Send a hwstatus message + * @param chan MAVLink channel to send the message + * + * @param Vcc board voltage (mV) + * @param I2Cerr I2C error count + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#else + mavlink_hwstatus_t packet; + packet.Vcc = Vcc; + packet.I2Cerr = I2Cerr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#else + mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf; + packet->Vcc = Vcc; + packet->I2Cerr = I2Cerr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HWSTATUS UNPACKING + + +/** + * @brief Get field Vcc from hwstatus message + * + * @return board voltage (mV) + */ +static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field I2Cerr from hwstatus message + * + * @return I2C error count + */ +static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a hwstatus message into a struct + * + * @param msg The message to decode + * @param hwstatus C-struct to decode the message contents into + */ +static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus) +{ +#if MAVLINK_NEED_BYTE_SWAP + hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg); + hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg); +#else + memcpy(hwstatus, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_led_control.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_led_control.h new file mode 100644 index 0000000..847e206 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_led_control.h @@ -0,0 +1,321 @@ +// MESSAGE LED_CONTROL PACKING + +#define MAVLINK_MSG_ID_LED_CONTROL 186 + +typedef struct __mavlink_led_control_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t instance; ///< Instance (LED instance to control or 255 for all LEDs) + uint8_t pattern; ///< Pattern (see LED_PATTERN_ENUM) + uint8_t custom_len; ///< Custom Byte Length + uint8_t custom_bytes[24]; ///< Custom Bytes +} mavlink_led_control_t; + +#define MAVLINK_MSG_ID_LED_CONTROL_LEN 29 +#define MAVLINK_MSG_ID_186_LEN 29 + +#define MAVLINK_MSG_ID_LED_CONTROL_CRC 72 +#define MAVLINK_MSG_ID_186_CRC 72 + +#define MAVLINK_MSG_LED_CONTROL_FIELD_CUSTOM_BYTES_LEN 24 + +#define MAVLINK_MESSAGE_INFO_LED_CONTROL { \ + "LED_CONTROL", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_led_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_led_control_t, target_component) }, \ + { "instance", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_led_control_t, instance) }, \ + { "pattern", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_led_control_t, pattern) }, \ + { "custom_len", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_led_control_t, custom_len) }, \ + { "custom_bytes", NULL, MAVLINK_TYPE_UINT8_T, 24, 5, offsetof(mavlink_led_control_t, custom_bytes) }, \ + } \ +} + + +/** + * @brief Pack a led_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param instance Instance (LED instance to control or 255 for all LEDs) + * @param pattern Pattern (see LED_PATTERN_ENUM) + * @param custom_len Custom Byte Length + * @param custom_bytes Custom Bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_led_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t instance, uint8_t pattern, uint8_t custom_len, const uint8_t *custom_bytes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LED_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, instance); + _mav_put_uint8_t(buf, 3, pattern); + _mav_put_uint8_t(buf, 4, custom_len); + _mav_put_uint8_t_array(buf, 5, custom_bytes, 24); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#else + mavlink_led_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.instance = instance; + packet.pattern = pattern; + packet.custom_len = custom_len; + mav_array_memcpy(packet.custom_bytes, custom_bytes, sizeof(uint8_t)*24); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LED_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a led_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param instance Instance (LED instance to control or 255 for all LEDs) + * @param pattern Pattern (see LED_PATTERN_ENUM) + * @param custom_len Custom Byte Length + * @param custom_bytes Custom Bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_led_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t instance,uint8_t pattern,uint8_t custom_len,const uint8_t *custom_bytes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LED_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, instance); + _mav_put_uint8_t(buf, 3, pattern); + _mav_put_uint8_t(buf, 4, custom_len); + _mav_put_uint8_t_array(buf, 5, custom_bytes, 24); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#else + mavlink_led_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.instance = instance; + packet.pattern = pattern; + packet.custom_len = custom_len; + mav_array_memcpy(packet.custom_bytes, custom_bytes, sizeof(uint8_t)*24); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LED_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a led_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param led_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_led_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_led_control_t* led_control) +{ + return mavlink_msg_led_control_pack(system_id, component_id, msg, led_control->target_system, led_control->target_component, led_control->instance, led_control->pattern, led_control->custom_len, led_control->custom_bytes); +} + +/** + * @brief Encode a led_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param led_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_led_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_led_control_t* led_control) +{ + return mavlink_msg_led_control_pack_chan(system_id, component_id, chan, msg, led_control->target_system, led_control->target_component, led_control->instance, led_control->pattern, led_control->custom_len, led_control->custom_bytes); +} + +/** + * @brief Send a led_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param instance Instance (LED instance to control or 255 for all LEDs) + * @param pattern Pattern (see LED_PATTERN_ENUM) + * @param custom_len Custom Byte Length + * @param custom_bytes Custom Bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_led_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t instance, uint8_t pattern, uint8_t custom_len, const uint8_t *custom_bytes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LED_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, instance); + _mav_put_uint8_t(buf, 3, pattern); + _mav_put_uint8_t(buf, 4, custom_len); + _mav_put_uint8_t_array(buf, 5, custom_bytes, 24); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, buf, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, buf, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +#else + mavlink_led_control_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.instance = instance; + packet.pattern = pattern; + packet.custom_len = custom_len; + mav_array_memcpy(packet.custom_bytes, custom_bytes, sizeof(uint8_t)*24); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LED_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_led_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t instance, uint8_t pattern, uint8_t custom_len, const uint8_t *custom_bytes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, instance); + _mav_put_uint8_t(buf, 3, pattern); + _mav_put_uint8_t(buf, 4, custom_len); + _mav_put_uint8_t_array(buf, 5, custom_bytes, 24); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, buf, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, buf, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +#else + mavlink_led_control_t *packet = (mavlink_led_control_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->instance = instance; + packet->pattern = pattern; + packet->custom_len = custom_len; + mav_array_memcpy(packet->custom_bytes, custom_bytes, sizeof(uint8_t)*24); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, (const char *)packet, MAVLINK_MSG_ID_LED_CONTROL_LEN, MAVLINK_MSG_ID_LED_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LED_CONTROL, (const char *)packet, MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LED_CONTROL UNPACKING + + +/** + * @brief Get field target_system from led_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_led_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from led_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_led_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field instance from led_control message + * + * @return Instance (LED instance to control or 255 for all LEDs) + */ +static inline uint8_t mavlink_msg_led_control_get_instance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field pattern from led_control message + * + * @return Pattern (see LED_PATTERN_ENUM) + */ +static inline uint8_t mavlink_msg_led_control_get_pattern(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field custom_len from led_control message + * + * @return Custom Byte Length + */ +static inline uint8_t mavlink_msg_led_control_get_custom_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field custom_bytes from led_control message + * + * @return Custom Bytes + */ +static inline uint16_t mavlink_msg_led_control_get_custom_bytes(const mavlink_message_t* msg, uint8_t *custom_bytes) +{ + return _MAV_RETURN_uint8_t_array(msg, custom_bytes, 24, 5); +} + +/** + * @brief Decode a led_control message into a struct + * + * @param msg The message to decode + * @param led_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_led_control_decode(const mavlink_message_t* msg, mavlink_led_control_t* led_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + led_control->target_system = mavlink_msg_led_control_get_target_system(msg); + led_control->target_component = mavlink_msg_led_control_get_target_component(msg); + led_control->instance = mavlink_msg_led_control_get_instance(msg); + led_control->pattern = mavlink_msg_led_control_get_pattern(msg); + led_control->custom_len = mavlink_msg_led_control_get_custom_len(msg); + mavlink_msg_led_control_get_custom_bytes(msg, led_control->custom_bytes); +#else + memcpy(led_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LED_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_limits_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_limits_status.h new file mode 100644 index 0000000..5fef937 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_limits_status.h @@ -0,0 +1,401 @@ +// MESSAGE LIMITS_STATUS PACKING + +#define MAVLINK_MSG_ID_LIMITS_STATUS 167 + +typedef struct __mavlink_limits_status_t +{ + uint32_t last_trigger; ///< time of last breach in milliseconds since boot + uint32_t last_action; ///< time of last recovery action in milliseconds since boot + uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot + uint32_t last_clear; ///< time of last all-clear in milliseconds since boot + uint16_t breach_count; ///< number of fence breaches + uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE) + uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) +} mavlink_limits_status_t; + +#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22 +#define MAVLINK_MSG_ID_167_LEN 22 + +#define MAVLINK_MSG_ID_LIMITS_STATUS_CRC 144 +#define MAVLINK_MSG_ID_167_CRC 144 + + + +#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \ + "LIMITS_STATUS", \ + 9, \ + { { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_limits_status_t, last_trigger) }, \ + { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_limits_status_t, last_action) }, \ + { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_limits_status_t, last_recovery) }, \ + { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_limits_status_t, last_clear) }, \ + { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_limits_status_t, breach_count) }, \ + { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_limits_status_t, limits_state) }, \ + { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \ + { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \ + { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \ + } \ +} + + +/** + * @brief Pack a limits_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#else + mavlink_limits_status_t packet; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.limits_state = limits_state; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +} + +/** + * @brief Pack a limits_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#else + mavlink_limits_status_t packet; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.limits_state = limits_state; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +} + +/** + * @brief Encode a limits_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** + * @brief Encode a limits_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** + * @brief Send a limits_status message + * @param chan MAVLink channel to send the message + * + * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#else + mavlink_limits_status_t packet; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.limits_state = limits_state; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LIMITS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_limits_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#else + mavlink_limits_status_t *packet = (mavlink_limits_status_t *)msgbuf; + packet->last_trigger = last_trigger; + packet->last_action = last_action; + packet->last_recovery = last_recovery; + packet->last_clear = last_clear; + packet->breach_count = breach_count; + packet->limits_state = limits_state; + packet->mods_enabled = mods_enabled; + packet->mods_required = mods_required; + packet->mods_triggered = mods_triggered; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LIMITS_STATUS UNPACKING + + +/** + * @brief Get field limits_state from limits_status message + * + * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE) + */ +static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field last_trigger from limits_status message + * + * @return time of last breach in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field last_action from limits_status message + * + * @return time of last recovery action in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field last_recovery from limits_status message + * + * @return time of last successful recovery in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field last_clear from limits_status message + * + * @return time of last all-clear in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field breach_count from limits_status message + * + * @return number of fence breaches + */ +static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field mods_enabled from limits_status message + * + * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field mods_required from limits_status message + * + * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field mods_triggered from limits_status message + * + * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a limits_status message into a struct + * + * @param msg The message to decode + * @param limits_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg); + limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg); + limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg); + limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg); + limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg); + limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg); + limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg); + limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg); + limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg); +#else + memcpy(limits_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_progress.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_progress.h new file mode 100644 index 0000000..c15853f --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_progress.h @@ -0,0 +1,393 @@ +// MESSAGE MAG_CAL_PROGRESS PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS 191 + +typedef struct __mavlink_mag_cal_progress_t +{ + float direction_x; ///< Body frame direction vector for display + float direction_y; ///< Body frame direction vector for display + float direction_z; ///< Body frame direction vector for display + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t attempt; ///< Attempt number + uint8_t completion_pct; ///< Completion percentage + uint8_t completion_mask[10]; ///< Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) +} mavlink_mag_cal_progress_t; + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN 27 +#define MAVLINK_MSG_ID_191_LEN 27 + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC 92 +#define MAVLINK_MSG_ID_191_CRC 92 + +#define MAVLINK_MSG_MAG_CAL_PROGRESS_FIELD_COMPLETION_MASK_LEN 10 + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_PROGRESS { \ + "MAG_CAL_PROGRESS", \ + 9, \ + { { "direction_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_progress_t, direction_x) }, \ + { "direction_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_progress_t, direction_y) }, \ + { "direction_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_progress_t, direction_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mag_cal_progress_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mag_cal_progress_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mag_cal_progress_t, cal_status) }, \ + { "attempt", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_mag_cal_progress_t, attempt) }, \ + { "completion_pct", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_mag_cal_progress_t, completion_pct) }, \ + { "completion_mask", NULL, MAVLINK_TYPE_UINT8_T, 10, 17, offsetof(mavlink_mag_cal_progress_t, completion_mask) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_progress message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_progress message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t attempt,uint8_t completion_pct,const uint8_t *completion_mask,float direction_x,float direction_y,float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_progress struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack(system_id, component_id, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Encode a mag_cal_progress struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack_chan(system_id, component_id, chan, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Send a mag_cal_progress message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_progress_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_progress_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t *packet = (mavlink_mag_cal_progress_t *)msgbuf; + packet->direction_x = direction_x; + packet->direction_y = direction_y; + packet->direction_z = direction_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->attempt = attempt; + packet->completion_pct = completion_pct; + mav_array_memcpy(packet->completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_PROGRESS UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_progress message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field cal_mask from mag_cal_progress message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field cal_status from mag_cal_progress message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field attempt from mag_cal_progress message + * + * @return Attempt number + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_attempt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field completion_pct from mag_cal_progress message + * + * @return Completion percentage + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_completion_pct(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field completion_mask from mag_cal_progress message + * + * @return Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_get_completion_mask(const mavlink_message_t* msg, uint8_t *completion_mask) +{ + return _MAV_RETURN_uint8_t_array(msg, completion_mask, 10, 17); +} + +/** + * @brief Get field direction_x from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field direction_y from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field direction_z from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a mag_cal_progress message into a struct + * + * @param msg The message to decode + * @param mag_cal_progress C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_progress_decode(const mavlink_message_t* msg, mavlink_mag_cal_progress_t* mag_cal_progress) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_progress->direction_x = mavlink_msg_mag_cal_progress_get_direction_x(msg); + mag_cal_progress->direction_y = mavlink_msg_mag_cal_progress_get_direction_y(msg); + mag_cal_progress->direction_z = mavlink_msg_mag_cal_progress_get_direction_z(msg); + mag_cal_progress->compass_id = mavlink_msg_mag_cal_progress_get_compass_id(msg); + mag_cal_progress->cal_mask = mavlink_msg_mag_cal_progress_get_cal_mask(msg); + mag_cal_progress->cal_status = mavlink_msg_mag_cal_progress_get_cal_status(msg); + mag_cal_progress->attempt = mavlink_msg_mag_cal_progress_get_attempt(msg); + mag_cal_progress->completion_pct = mavlink_msg_mag_cal_progress_get_completion_pct(msg); + mavlink_msg_mag_cal_progress_get_completion_mask(msg, mag_cal_progress->completion_mask); +#else + memcpy(mag_cal_progress, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_report.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_report.h new file mode 100644 index 0000000..f8056b7 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mag_cal_report.h @@ -0,0 +1,521 @@ +// MESSAGE MAG_CAL_REPORT PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 + +typedef struct __mavlink_mag_cal_report_t +{ + float fitness; ///< RMS milligauss residuals + float ofs_x; ///< X offset + float ofs_y; ///< Y offset + float ofs_z; ///< Z offset + float diag_x; ///< X diagonal (matrix 11) + float diag_y; ///< Y diagonal (matrix 22) + float diag_z; ///< Z diagonal (matrix 33) + float offdiag_x; ///< X off-diagonal (matrix 12 and 21) + float offdiag_y; ///< Y off-diagonal (matrix 13 and 31) + float offdiag_z; ///< Z off-diagonal (matrix 32 and 23) + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t autosaved; ///< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters +} mavlink_mag_cal_report_t; + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44 +#define MAVLINK_MSG_ID_192_LEN 44 + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 +#define MAVLINK_MSG_ID_192_CRC 36 + + + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + "MAG_CAL_REPORT", \ + 14, \ + { { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Encode a mag_cal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t *packet = (mavlink_mag_cal_report_t *)msgbuf; + packet->fitness = fitness; + packet->ofs_x = ofs_x; + packet->ofs_y = ofs_y; + packet->ofs_z = ofs_z; + packet->diag_x = diag_x; + packet->diag_y = diag_y; + packet->diag_z = diag_z; + packet->offdiag_x = offdiag_x; + packet->offdiag_y = offdiag_y; + packet->offdiag_z = offdiag_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_REPORT UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_report message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field cal_mask from mag_cal_report message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field cal_status from mag_cal_report message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field autosaved from mag_cal_report message + * + * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field fitness from mag_cal_report message + * + * @return RMS milligauss residuals + */ +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field ofs_x from mag_cal_report message + * + * @return X offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field ofs_y from mag_cal_report message + * + * @return Y offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field ofs_z from mag_cal_report message + * + * @return Z offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field diag_x from mag_cal_report message + * + * @return X diagonal (matrix 11) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field diag_y from mag_cal_report message + * + * @return Y diagonal (matrix 22) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field diag_z from mag_cal_report message + * + * @return Z diagonal (matrix 33) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field offdiag_x from mag_cal_report message + * + * @return X off-diagonal (matrix 12 and 21) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field offdiag_y from mag_cal_report message + * + * @return Y off-diagonal (matrix 13 and 31) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field offdiag_z from mag_cal_report message + * + * @return Z off-diagonal (matrix 32 and 23) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a mag_cal_report message into a struct + * + * @param msg The message to decode + * @param mag_cal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); + mag_cal_report->ofs_x = mavlink_msg_mag_cal_report_get_ofs_x(msg); + mag_cal_report->ofs_y = mavlink_msg_mag_cal_report_get_ofs_y(msg); + mag_cal_report->ofs_z = mavlink_msg_mag_cal_report_get_ofs_z(msg); + mag_cal_report->diag_x = mavlink_msg_mag_cal_report_get_diag_x(msg); + mag_cal_report->diag_y = mavlink_msg_mag_cal_report_get_diag_y(msg); + mag_cal_report->diag_z = mavlink_msg_mag_cal_report_get_diag_z(msg); + mag_cal_report->offdiag_x = mavlink_msg_mag_cal_report_get_offdiag_x(msg); + mag_cal_report->offdiag_y = mavlink_msg_mag_cal_report_get_offdiag_y(msg); + mag_cal_report->offdiag_z = mavlink_msg_mag_cal_report_get_offdiag_z(msg); + mag_cal_report->compass_id = mavlink_msg_mag_cal_report_get_compass_id(msg); + mag_cal_report->cal_mask = mavlink_msg_mag_cal_report_get_cal_mask(msg); + mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); + mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); +#else + memcpy(mag_cal_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_meminfo.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_meminfo.h new file mode 100644 index 0000000..c64b2e9 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_meminfo.h @@ -0,0 +1,233 @@ +// MESSAGE MEMINFO PACKING + +#define MAVLINK_MSG_ID_MEMINFO 152 + +typedef struct __mavlink_meminfo_t +{ + uint16_t brkval; ///< heap top + uint16_t freemem; ///< free memory +} mavlink_meminfo_t; + +#define MAVLINK_MSG_ID_MEMINFO_LEN 4 +#define MAVLINK_MSG_ID_152_LEN 4 + +#define MAVLINK_MSG_ID_MEMINFO_CRC 208 +#define MAVLINK_MSG_ID_152_CRC 208 + + + +#define MAVLINK_MESSAGE_INFO_MEMINFO { \ + "MEMINFO", \ + 2, \ + { { "brkval", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_meminfo_t, brkval) }, \ + { "freemem", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_meminfo_t, freemem) }, \ + } \ +} + + +/** + * @brief Pack a meminfo message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param brkval heap top + * @param freemem free memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t brkval, uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#else + mavlink_meminfo_t packet; + packet.brkval = brkval; + packet.freemem = freemem; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MEMINFO; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +} + +/** + * @brief Pack a meminfo message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param brkval heap top + * @param freemem free memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t brkval,uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#else + mavlink_meminfo_t packet; + packet.brkval = brkval; + packet.freemem = freemem; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MEMINFO; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +} + +/** + * @brief Encode a meminfo struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param meminfo C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) +{ + return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem); +} + +/** + * @brief Encode a meminfo struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param meminfo C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) +{ + return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); +} + +/** + * @brief Send a meminfo message + * @param chan MAVLink channel to send the message + * + * @param brkval heap top + * @param freemem free memory + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#else + mavlink_meminfo_t packet; + packet.brkval = brkval; + packet.freemem = freemem; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MEMINFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_meminfo_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#else + mavlink_meminfo_t *packet = (mavlink_meminfo_t *)msgbuf; + packet->brkval = brkval; + packet->freemem = freemem; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MEMINFO UNPACKING + + +/** + * @brief Get field brkval from meminfo message + * + * @return heap top + */ +static inline uint16_t mavlink_msg_meminfo_get_brkval(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field freemem from meminfo message + * + * @return free memory + */ +static inline uint16_t mavlink_msg_meminfo_get_freemem(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a meminfo message into a struct + * + * @param msg The message to decode + * @param meminfo C-struct to decode the message contents into + */ +static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavlink_meminfo_t* meminfo) +{ +#if MAVLINK_NEED_BYTE_SWAP + meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg); + meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg); +#else + memcpy(meminfo, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_configure.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_configure.h new file mode 100644 index 0000000..350bb0b --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_configure.h @@ -0,0 +1,329 @@ +// MESSAGE MOUNT_CONFIGURE PACKING + +#define MAVLINK_MSG_ID_MOUNT_CONFIGURE 156 + +typedef struct __mavlink_mount_configure_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t mount_mode; ///< mount operating mode (see MAV_MOUNT_MODE enum) + uint8_t stab_roll; ///< (1 = yes, 0 = no) + uint8_t stab_pitch; ///< (1 = yes, 0 = no) + uint8_t stab_yaw; ///< (1 = yes, 0 = no) +} mavlink_mount_configure_t; + +#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN 6 +#define MAVLINK_MSG_ID_156_LEN 6 + +#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC 19 +#define MAVLINK_MSG_ID_156_CRC 19 + + + +#define MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE { \ + "MOUNT_CONFIGURE", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_configure_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_configure_t, target_component) }, \ + { "mount_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mount_configure_t, mount_mode) }, \ + { "stab_roll", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mount_configure_t, stab_roll) }, \ + { "stab_pitch", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mount_configure_t, stab_pitch) }, \ + { "stab_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mount_configure_t, stab_yaw) }, \ + } \ +} + + +/** + * @brief Pack a mount_configure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) + * @param stab_roll (1 = yes, 0 = no) + * @param stab_pitch (1 = yes, 0 = no) + * @param stab_yaw (1 = yes, 0 = no) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#else + mavlink_mount_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mount_mode = mount_mode; + packet.stab_roll = stab_roll; + packet.stab_pitch = stab_pitch; + packet.stab_yaw = stab_yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +} + +/** + * @brief Pack a mount_configure message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) + * @param stab_roll (1 = yes, 0 = no) + * @param stab_pitch (1 = yes, 0 = no) + * @param stab_yaw (1 = yes, 0 = no) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t mount_mode,uint8_t stab_roll,uint8_t stab_pitch,uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#else + mavlink_mount_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mount_mode = mount_mode; + packet.stab_roll = stab_roll; + packet.stab_pitch = stab_pitch; + packet.stab_yaw = stab_yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +} + +/** + * @brief Encode a mount_configure struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mount_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) +{ + return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); +} + +/** + * @brief Encode a mount_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) +{ + return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); +} + +/** + * @brief Send a mount_configure message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) + * @param stab_roll (1 = yes, 0 = no) + * @param stab_pitch (1 = yes, 0 = no) + * @param stab_yaw (1 = yes, 0 = no) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#else + mavlink_mount_configure_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mount_mode = mount_mode; + packet.stab_roll = stab_roll; + packet.stab_pitch = stab_pitch; + packet.stab_yaw = stab_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#else + mavlink_mount_configure_t *packet = (mavlink_mount_configure_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mount_mode = mount_mode; + packet->stab_roll = stab_roll; + packet->stab_pitch = stab_pitch; + packet->stab_yaw = stab_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MOUNT_CONFIGURE UNPACKING + + +/** + * @brief Get field target_system from mount_configure message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mount_configure_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mount_configure message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mount_configure_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field mount_mode from mount_configure message + * + * @return mount operating mode (see MAV_MOUNT_MODE enum) + */ +static inline uint8_t mavlink_msg_mount_configure_get_mount_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field stab_roll from mount_configure message + * + * @return (1 = yes, 0 = no) + */ +static inline uint8_t mavlink_msg_mount_configure_get_stab_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field stab_pitch from mount_configure message + * + * @return (1 = yes, 0 = no) + */ +static inline uint8_t mavlink_msg_mount_configure_get_stab_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field stab_yaw from mount_configure message + * + * @return (1 = yes, 0 = no) + */ +static inline uint8_t mavlink_msg_mount_configure_get_stab_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Decode a mount_configure message into a struct + * + * @param msg The message to decode + * @param mount_configure C-struct to decode the message contents into + */ +static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* msg, mavlink_mount_configure_t* mount_configure) +{ +#if MAVLINK_NEED_BYTE_SWAP + mount_configure->target_system = mavlink_msg_mount_configure_get_target_system(msg); + mount_configure->target_component = mavlink_msg_mount_configure_get_target_component(msg); + mount_configure->mount_mode = mavlink_msg_mount_configure_get_mount_mode(msg); + mount_configure->stab_roll = mavlink_msg_mount_configure_get_stab_roll(msg); + mount_configure->stab_pitch = mavlink_msg_mount_configure_get_stab_pitch(msg); + mount_configure->stab_yaw = mavlink_msg_mount_configure_get_stab_yaw(msg); +#else + memcpy(mount_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_control.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_control.h new file mode 100644 index 0000000..72d6e24 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_control.h @@ -0,0 +1,329 @@ +// MESSAGE MOUNT_CONTROL PACKING + +#define MAVLINK_MSG_ID_MOUNT_CONTROL 157 + +typedef struct __mavlink_mount_control_t +{ + int32_t input_a; ///< pitch(deg*100) or lat, depending on mount mode + int32_t input_b; ///< roll(deg*100) or lon depending on mount mode + int32_t input_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t save_position; ///< if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) +} mavlink_mount_control_t; + +#define MAVLINK_MSG_ID_MOUNT_CONTROL_LEN 15 +#define MAVLINK_MSG_ID_157_LEN 15 + +#define MAVLINK_MSG_ID_MOUNT_CONTROL_CRC 21 +#define MAVLINK_MSG_ID_157_CRC 21 + + + +#define MAVLINK_MESSAGE_INFO_MOUNT_CONTROL { \ + "MOUNT_CONTROL", \ + 6, \ + { { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_control_t, input_a) }, \ + { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_control_t, input_b) }, \ + { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_control_t, input_c) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_control_t, target_component) }, \ + { "save_position", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mount_control_t, save_position) }, \ + } \ +} + + +/** + * @brief Pack a mount_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param input_a pitch(deg*100) or lat, depending on mount mode + * @param input_b roll(deg*100) or lon depending on mount mode + * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode + * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#else + mavlink_mount_control_t packet; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.target_system = target_system; + packet.target_component = target_component; + packet.save_position = save_position; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a mount_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param input_a pitch(deg*100) or lat, depending on mount mode + * @param input_b roll(deg*100) or lon depending on mount mode + * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode + * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int32_t input_a,int32_t input_b,int32_t input_c,uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#else + mavlink_mount_control_t packet; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.target_system = target_system; + packet.target_component = target_component; + packet.save_position = save_position; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a mount_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mount_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) +{ + return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); +} + +/** + * @brief Encode a mount_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) +{ + return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); +} + +/** + * @brief Send a mount_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param input_a pitch(deg*100) or lat, depending on mount mode + * @param input_b roll(deg*100) or lon depending on mount mode + * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode + * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#else + mavlink_mount_control_t packet; + packet.input_a = input_a; + packet.input_b = input_b; + packet.input_c = input_c; + packet.target_system = target_system; + packet.target_component = target_component; + packet.save_position = save_position; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MOUNT_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#else + mavlink_mount_control_t *packet = (mavlink_mount_control_t *)msgbuf; + packet->input_a = input_a; + packet->input_b = input_b; + packet->input_c = input_c; + packet->target_system = target_system; + packet->target_component = target_component; + packet->save_position = save_position; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MOUNT_CONTROL UNPACKING + + +/** + * @brief Get field target_system from mount_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mount_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field target_component from mount_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mount_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field input_a from mount_control message + * + * @return pitch(deg*100) or lat, depending on mount mode + */ +static inline int32_t mavlink_msg_mount_control_get_input_a(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field input_b from mount_control message + * + * @return roll(deg*100) or lon depending on mount mode + */ +static inline int32_t mavlink_msg_mount_control_get_input_b(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field input_c from mount_control message + * + * @return yaw(deg*100) or alt (in cm) depending on mount mode + */ +static inline int32_t mavlink_msg_mount_control_get_input_c(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field save_position from mount_control message + * + * @return if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + */ +static inline uint8_t mavlink_msg_mount_control_get_save_position(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Decode a mount_control message into a struct + * + * @param msg The message to decode + * @param mount_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* msg, mavlink_mount_control_t* mount_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + mount_control->input_a = mavlink_msg_mount_control_get_input_a(msg); + mount_control->input_b = mavlink_msg_mount_control_get_input_b(msg); + mount_control->input_c = mavlink_msg_mount_control_get_input_c(msg); + mount_control->target_system = mavlink_msg_mount_control_get_target_system(msg); + mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg); + mount_control->save_position = mavlink_msg_mount_control_get_save_position(msg); +#else + memcpy(mount_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_status.h new file mode 100644 index 0000000..5c4fa0c --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_mount_status.h @@ -0,0 +1,305 @@ +// MESSAGE MOUNT_STATUS PACKING + +#define MAVLINK_MSG_ID_MOUNT_STATUS 158 + +typedef struct __mavlink_mount_status_t +{ + int32_t pointing_a; ///< pitch(deg*100) + int32_t pointing_b; ///< roll(deg*100) + int32_t pointing_c; ///< yaw(deg*100) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mount_status_t; + +#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14 +#define MAVLINK_MSG_ID_158_LEN 14 + +#define MAVLINK_MSG_ID_MOUNT_STATUS_CRC 134 +#define MAVLINK_MSG_ID_158_CRC 134 + + + +#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \ + "MOUNT_STATUS", \ + 5, \ + { { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_status_t, pointing_a) }, \ + { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_status_t, pointing_b) }, \ + { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_status_t, pointing_c) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_status_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mount_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param pointing_a pitch(deg*100) + * @param pointing_b roll(deg*100) + * @param pointing_c yaw(deg*100) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#else + mavlink_mount_status_t packet; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +} + +/** + * @brief Pack a mount_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param pointing_a pitch(deg*100) + * @param pointing_b roll(deg*100) + * @param pointing_c yaw(deg*100) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int32_t pointing_a,int32_t pointing_b,int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#else + mavlink_mount_status_t packet; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +} + +/** + * @brief Encode a mount_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mount_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) +{ + return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); +} + +/** + * @brief Encode a mount_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) +{ + return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); +} + +/** + * @brief Send a mount_status message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param pointing_a pitch(deg*100) + * @param pointing_b roll(deg*100) + * @param pointing_c yaw(deg*100) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#else + mavlink_mount_status_t packet; + packet.pointing_a = pointing_a; + packet.pointing_b = pointing_b; + packet.pointing_c = pointing_c; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MOUNT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#else + mavlink_mount_status_t *packet = (mavlink_mount_status_t *)msgbuf; + packet->pointing_a = pointing_a; + packet->pointing_b = pointing_b; + packet->pointing_c = pointing_c; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MOUNT_STATUS UNPACKING + + +/** + * @brief Get field target_system from mount_status message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mount_status_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field target_component from mount_status message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mount_status_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field pointing_a from mount_status message + * + * @return pitch(deg*100) + */ +static inline int32_t mavlink_msg_mount_status_get_pointing_a(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field pointing_b from mount_status message + * + * @return roll(deg*100) + */ +static inline int32_t mavlink_msg_mount_status_get_pointing_b(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field pointing_c from mount_status message + * + * @return yaw(deg*100) + */ +static inline int32_t mavlink_msg_mount_status_get_pointing_c(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Decode a mount_status message into a struct + * + * @param msg The message to decode + * @param mount_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* msg, mavlink_mount_status_t* mount_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + mount_status->pointing_a = mavlink_msg_mount_status_get_pointing_a(msg); + mount_status->pointing_b = mavlink_msg_mount_status_get_pointing_b(msg); + mount_status->pointing_c = mavlink_msg_mount_status_get_pointing_c(msg); + mount_status->target_system = mavlink_msg_mount_status_get_target_system(msg); + mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg); +#else + memcpy(mount_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_pid_tuning.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_pid_tuning.h new file mode 100644 index 0000000..0066498 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_pid_tuning.h @@ -0,0 +1,353 @@ +// MESSAGE PID_TUNING PACKING + +#define MAVLINK_MSG_ID_PID_TUNING 194 + +typedef struct __mavlink_pid_tuning_t +{ + float desired; ///< desired rate (degrees/s) + float achieved; ///< achieved rate (degrees/s) + float FF; ///< FF component + float P; ///< P component + float I; ///< I component + float D; ///< D component + uint8_t axis; ///< axis +} mavlink_pid_tuning_t; + +#define MAVLINK_MSG_ID_PID_TUNING_LEN 25 +#define MAVLINK_MSG_ID_194_LEN 25 + +#define MAVLINK_MSG_ID_PID_TUNING_CRC 98 +#define MAVLINK_MSG_ID_194_CRC 98 + + + +#define MAVLINK_MESSAGE_INFO_PID_TUNING { \ + "PID_TUNING", \ + 7, \ + { { "desired", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pid_tuning_t, desired) }, \ + { "achieved", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pid_tuning_t, achieved) }, \ + { "FF", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_pid_tuning_t, FF) }, \ + { "P", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_pid_tuning_t, P) }, \ + { "I", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_pid_tuning_t, I) }, \ + { "D", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_pid_tuning_t, D) }, \ + { "axis", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_pid_tuning_t, axis) }, \ + } \ +} + + +/** + * @brief Pack a pid_tuning message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param axis axis + * @param desired desired rate (degrees/s) + * @param achieved achieved rate (degrees/s) + * @param FF FF component + * @param P P component + * @param I I component + * @param D D component + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_pid_tuning_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t axis, float desired, float achieved, float FF, float P, float I, float D) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PID_TUNING_LEN]; + _mav_put_float(buf, 0, desired); + _mav_put_float(buf, 4, achieved); + _mav_put_float(buf, 8, FF); + _mav_put_float(buf, 12, P); + _mav_put_float(buf, 16, I); + _mav_put_float(buf, 20, D); + _mav_put_uint8_t(buf, 24, axis); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PID_TUNING_LEN); +#else + mavlink_pid_tuning_t packet; + packet.desired = desired; + packet.achieved = achieved; + packet.FF = FF; + packet.P = P; + packet.I = I; + packet.D = D; + packet.axis = axis; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PID_TUNING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +} + +/** + * @brief Pack a pid_tuning message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param axis axis + * @param desired desired rate (degrees/s) + * @param achieved achieved rate (degrees/s) + * @param FF FF component + * @param P P component + * @param I I component + * @param D D component + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_pid_tuning_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t axis,float desired,float achieved,float FF,float P,float I,float D) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PID_TUNING_LEN]; + _mav_put_float(buf, 0, desired); + _mav_put_float(buf, 4, achieved); + _mav_put_float(buf, 8, FF); + _mav_put_float(buf, 12, P); + _mav_put_float(buf, 16, I); + _mav_put_float(buf, 20, D); + _mav_put_uint8_t(buf, 24, axis); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PID_TUNING_LEN); +#else + mavlink_pid_tuning_t packet; + packet.desired = desired; + packet.achieved = achieved; + packet.FF = FF; + packet.P = P; + packet.I = I; + packet.D = D; + packet.axis = axis; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PID_TUNING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +} + +/** + * @brief Encode a pid_tuning struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param pid_tuning C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pid_tuning_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pid_tuning_t* pid_tuning) +{ + return mavlink_msg_pid_tuning_pack(system_id, component_id, msg, pid_tuning->axis, pid_tuning->desired, pid_tuning->achieved, pid_tuning->FF, pid_tuning->P, pid_tuning->I, pid_tuning->D); +} + +/** + * @brief Encode a pid_tuning struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pid_tuning C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pid_tuning_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pid_tuning_t* pid_tuning) +{ + return mavlink_msg_pid_tuning_pack_chan(system_id, component_id, chan, msg, pid_tuning->axis, pid_tuning->desired, pid_tuning->achieved, pid_tuning->FF, pid_tuning->P, pid_tuning->I, pid_tuning->D); +} + +/** + * @brief Send a pid_tuning message + * @param chan MAVLink channel to send the message + * + * @param axis axis + * @param desired desired rate (degrees/s) + * @param achieved achieved rate (degrees/s) + * @param FF FF component + * @param P P component + * @param I I component + * @param D D component + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_pid_tuning_send(mavlink_channel_t chan, uint8_t axis, float desired, float achieved, float FF, float P, float I, float D) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PID_TUNING_LEN]; + _mav_put_float(buf, 0, desired); + _mav_put_float(buf, 4, achieved); + _mav_put_float(buf, 8, FF); + _mav_put_float(buf, 12, P); + _mav_put_float(buf, 16, I); + _mav_put_float(buf, 20, D); + _mav_put_uint8_t(buf, 24, axis); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, buf, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, buf, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +#else + mavlink_pid_tuning_t packet; + packet.desired = desired; + packet.achieved = achieved; + packet.FF = FF; + packet.P = P; + packet.I = I; + packet.D = D; + packet.axis = axis; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, (const char *)&packet, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, (const char *)&packet, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PID_TUNING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_pid_tuning_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t axis, float desired, float achieved, float FF, float P, float I, float D) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, desired); + _mav_put_float(buf, 4, achieved); + _mav_put_float(buf, 8, FF); + _mav_put_float(buf, 12, P); + _mav_put_float(buf, 16, I); + _mav_put_float(buf, 20, D); + _mav_put_uint8_t(buf, 24, axis); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, buf, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, buf, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +#else + mavlink_pid_tuning_t *packet = (mavlink_pid_tuning_t *)msgbuf; + packet->desired = desired; + packet->achieved = achieved; + packet->FF = FF; + packet->P = P; + packet->I = I; + packet->D = D; + packet->axis = axis; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, (const char *)packet, MAVLINK_MSG_ID_PID_TUNING_LEN, MAVLINK_MSG_ID_PID_TUNING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PID_TUNING, (const char *)packet, MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PID_TUNING UNPACKING + + +/** + * @brief Get field axis from pid_tuning message + * + * @return axis + */ +static inline uint8_t mavlink_msg_pid_tuning_get_axis(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field desired from pid_tuning message + * + * @return desired rate (degrees/s) + */ +static inline float mavlink_msg_pid_tuning_get_desired(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field achieved from pid_tuning message + * + * @return achieved rate (degrees/s) + */ +static inline float mavlink_msg_pid_tuning_get_achieved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field FF from pid_tuning message + * + * @return FF component + */ +static inline float mavlink_msg_pid_tuning_get_FF(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field P from pid_tuning message + * + * @return P component + */ +static inline float mavlink_msg_pid_tuning_get_P(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field I from pid_tuning message + * + * @return I component + */ +static inline float mavlink_msg_pid_tuning_get_I(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field D from pid_tuning message + * + * @return D component + */ +static inline float mavlink_msg_pid_tuning_get_D(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a pid_tuning message into a struct + * + * @param msg The message to decode + * @param pid_tuning C-struct to decode the message contents into + */ +static inline void mavlink_msg_pid_tuning_decode(const mavlink_message_t* msg, mavlink_pid_tuning_t* pid_tuning) +{ +#if MAVLINK_NEED_BYTE_SWAP + pid_tuning->desired = mavlink_msg_pid_tuning_get_desired(msg); + pid_tuning->achieved = mavlink_msg_pid_tuning_get_achieved(msg); + pid_tuning->FF = mavlink_msg_pid_tuning_get_FF(msg); + pid_tuning->P = mavlink_msg_pid_tuning_get_P(msg); + pid_tuning->I = mavlink_msg_pid_tuning_get_I(msg); + pid_tuning->D = mavlink_msg_pid_tuning_get_D(msg); + pid_tuning->axis = mavlink_msg_pid_tuning_get_axis(msg); +#else + memcpy(pid_tuning, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PID_TUNING_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_radio.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_radio.h new file mode 100644 index 0000000..830eb46 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_radio.h @@ -0,0 +1,353 @@ +// MESSAGE RADIO PACKING + +#define MAVLINK_MSG_ID_RADIO 166 + +typedef struct __mavlink_radio_t +{ + uint16_t rxerrors; ///< receive errors + uint16_t fixed; ///< count of error corrected packets + uint8_t rssi; ///< local signal strength + uint8_t remrssi; ///< remote signal strength + uint8_t txbuf; ///< how full the tx buffer is as a percentage + uint8_t noise; ///< background noise level + uint8_t remnoise; ///< remote background noise level +} mavlink_radio_t; + +#define MAVLINK_MSG_ID_RADIO_LEN 9 +#define MAVLINK_MSG_ID_166_LEN 9 + +#define MAVLINK_MSG_ID_RADIO_CRC 21 +#define MAVLINK_MSG_ID_166_CRC 21 + + + +#define MAVLINK_MESSAGE_INFO_RADIO { \ + "RADIO", \ + 7, \ + { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \ + { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, fixed) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_t, rssi) }, \ + { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_t, remrssi) }, \ + { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \ + { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, noise) }, \ + { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, remnoise) }, \ + } \ +} + + +/** + * @brief Pack a radio message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); +#else + mavlink_radio_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN); +#endif +} + +/** + * @brief Pack a radio message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); +#else + mavlink_radio_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN); +#endif +} + +/** + * @brief Encode a radio struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); +} + +/** + * @brief Encode a radio struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); +} + +/** + * @brief Send a radio message + * @param chan MAVLink channel to send the message + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#else + mavlink_radio_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RADIO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#else + mavlink_radio_t *packet = (mavlink_radio_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RADIO UNPACKING + + +/** + * @brief Get field rssi from radio message + * + * @return local signal strength + */ +static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field remrssi from radio message + * + * @return remote signal strength + */ +static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field txbuf from radio message + * + * @return how full the tx buffer is as a percentage + */ +static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field noise from radio message + * + * @return background noise level + */ +static inline uint8_t mavlink_msg_radio_get_noise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field remnoise from radio message + * + * @return remote background noise level + */ +static inline uint8_t mavlink_msg_radio_get_remnoise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field rxerrors from radio message + * + * @return receive errors + */ +static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field fixed from radio message + * + * @return count of error corrected packets + */ +static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a radio message into a struct + * + * @param msg The message to decode + * @param radio C-struct to decode the message contents into + */ +static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio) +{ +#if MAVLINK_NEED_BYTE_SWAP + radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg); + radio->fixed = mavlink_msg_radio_get_fixed(msg); + radio->rssi = mavlink_msg_radio_get_rssi(msg); + radio->remrssi = mavlink_msg_radio_get_remrssi(msg); + radio->txbuf = mavlink_msg_radio_get_txbuf(msg); + radio->noise = mavlink_msg_radio_get_noise(msg); + radio->remnoise = mavlink_msg_radio_get_remnoise(msg); +#else + memcpy(radio, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_fetch_point.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_fetch_point.h new file mode 100644 index 0000000..4598251 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_fetch_point.h @@ -0,0 +1,257 @@ +// MESSAGE RALLY_FETCH_POINT PACKING + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176 + +typedef struct __mavlink_rally_fetch_point_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 0) +} mavlink_rally_fetch_point_t; + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3 +#define MAVLINK_MSG_ID_176_LEN 3 + +#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234 +#define MAVLINK_MSG_ID_176_CRC 234 + + + +#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \ + "RALLY_FETCH_POINT", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \ + } \ +} + + +/** + * @brief Pack a rally_fetch_point message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Pack a rally_fetch_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} + +/** + * @brief Encode a rally_fetch_point struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rally_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) +{ + return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); +} + +/** + * @brief Encode a rally_fetch_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rally_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) +{ + return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); +} + +/** + * @brief Send a rally_fetch_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#else + mavlink_rally_fetch_point_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#else + mavlink_rally_fetch_point_t *packet = (mavlink_rally_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RALLY_FETCH_POINT UNPACKING + + +/** + * @brief Get field target_system from rally_fetch_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from rally_fetch_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field idx from rally_fetch_point message + * + * @return point index (first point is 0) + */ +static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a rally_fetch_point message into a struct + * + * @param msg The message to decode + * @param rally_fetch_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg); + rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg); + rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg); +#else + memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_point.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_point.h new file mode 100644 index 0000000..b44e764 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rally_point.h @@ -0,0 +1,425 @@ +// MESSAGE RALLY_POINT PACKING + +#define MAVLINK_MSG_ID_RALLY_POINT 175 + +typedef struct __mavlink_rally_point_t +{ + int32_t lat; ///< Latitude of point in degrees * 1E7 + int32_t lng; ///< Longitude of point in degrees * 1E7 + int16_t alt; ///< Transit / loiter altitude in meters relative to home + int16_t break_alt; ///< Break altitude in meters relative to home + uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t idx; ///< point index (first point is 0) + uint8_t count; ///< total number of points (for sanity checking) + uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask. +} mavlink_rally_point_t; + +#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19 +#define MAVLINK_MSG_ID_175_LEN 19 + +#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138 +#define MAVLINK_MSG_ID_175_CRC 138 + + + +#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \ + "RALLY_POINT", \ + 10, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \ + { "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \ + { "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \ + { "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \ + { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a rally_point message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} + +/** + * @brief Pack a rally_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,int32_t lat,int32_t lng,int16_t alt,int16_t break_alt,uint16_t land_dir,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} + +/** + * @brief Encode a rally_point struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rally_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) +{ + return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); +} + +/** + * @brief Encode a rally_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rally_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) +{ + return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); +} + +/** + * @brief Send a rally_point message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param idx point index (first point is 0) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point in degrees * 1E7 + * @param lng Longitude of point in degrees * 1E7 + * @param alt Transit / loiter altitude in meters relative to home + * @param break_alt Break altitude in meters relative to home + * @param land_dir Heading to aim for when landing. In centi-degrees. + * @param flags See RALLY_FLAGS enum for definition of the bitmask. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#else + mavlink_rally_point_t packet; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.break_alt = break_alt; + packet.land_dir = land_dir; + packet.target_system = target_system; + packet.target_component = target_component; + packet.idx = idx; + packet.count = count; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RALLY_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#else + mavlink_rally_point_t *packet = (mavlink_rally_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->alt = alt; + packet->break_alt = break_alt; + packet->land_dir = land_dir; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RALLY_POINT UNPACKING + + +/** + * @brief Get field target_system from rally_point message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field target_component from rally_point message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field idx from rally_point message + * + * @return point index (first point is 0) + */ +static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field count from rally_point message + * + * @return total number of points (for sanity checking) + */ +static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field lat from rally_point message + * + * @return Latitude of point in degrees * 1E7 + */ +static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lng from rally_point message + * + * @return Longitude of point in degrees * 1E7 + */ +static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field alt from rally_point message + * + * @return Transit / loiter altitude in meters relative to home + */ +static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field break_alt from rally_point message + * + * @return Break altitude in meters relative to home + */ +static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field land_dir from rally_point message + * + * @return Heading to aim for when landing. In centi-degrees. + */ +static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field flags from rally_point message + * + * @return See RALLY_FLAGS enum for definition of the bitmask. + */ +static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Decode a rally_point message into a struct + * + * @param msg The message to decode + * @param rally_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + rally_point->lat = mavlink_msg_rally_point_get_lat(msg); + rally_point->lng = mavlink_msg_rally_point_get_lng(msg); + rally_point->alt = mavlink_msg_rally_point_get_alt(msg); + rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg); + rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg); + rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg); + rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg); + rally_point->idx = mavlink_msg_rally_point_get_idx(msg); + rally_point->count = mavlink_msg_rally_point_get_count(msg); + rally_point->flags = mavlink_msg_rally_point_get_flags(msg); +#else + memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rangefinder.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rangefinder.h new file mode 100644 index 0000000..464ce8a --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_rangefinder.h @@ -0,0 +1,233 @@ +// MESSAGE RANGEFINDER PACKING + +#define MAVLINK_MSG_ID_RANGEFINDER 173 + +typedef struct __mavlink_rangefinder_t +{ + float distance; ///< distance in meters + float voltage; ///< raw voltage if available, zero otherwise +} mavlink_rangefinder_t; + +#define MAVLINK_MSG_ID_RANGEFINDER_LEN 8 +#define MAVLINK_MSG_ID_173_LEN 8 + +#define MAVLINK_MSG_ID_RANGEFINDER_CRC 83 +#define MAVLINK_MSG_ID_173_CRC 83 + + + +#define MAVLINK_MESSAGE_INFO_RANGEFINDER { \ + "RANGEFINDER", \ + 2, \ + { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \ + } \ +} + + +/** + * @brief Pack a rangefinder message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param distance distance in meters + * @param voltage raw voltage if available, zero otherwise + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float distance, float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#else + mavlink_rangefinder_t packet; + packet.distance = distance; + packet.voltage = voltage; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +} + +/** + * @brief Pack a rangefinder message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param distance distance in meters + * @param voltage raw voltage if available, zero otherwise + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float distance,float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#else + mavlink_rangefinder_t packet; + packet.distance = distance; + packet.voltage = voltage; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +} + +/** + * @brief Encode a rangefinder struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rangefinder C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) +{ + return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage); +} + +/** + * @brief Encode a rangefinder struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rangefinder C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) +{ + return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); +} + +/** + * @brief Send a rangefinder message + * @param chan MAVLink channel to send the message + * + * @param distance distance in meters + * @param voltage raw voltage if available, zero otherwise + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#else + mavlink_rangefinder_t packet; + packet.distance = distance; + packet.voltage = voltage; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float distance, float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#else + mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf; + packet->distance = distance; + packet->voltage = voltage; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RANGEFINDER UNPACKING + + +/** + * @brief Get field distance from rangefinder message + * + * @return distance in meters + */ +static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field voltage from rangefinder message + * + * @return raw voltage if available, zero otherwise + */ +static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Decode a rangefinder message into a struct + * + * @param msg The message to decode + * @param rangefinder C-struct to decode the message contents into + */ +static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder) +{ +#if MAVLINK_NEED_BYTE_SWAP + rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg); + rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg); +#else + memcpy(rangefinder, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_block_status.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_block_status.h new file mode 100644 index 0000000..6c4d69b --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_block_status.h @@ -0,0 +1,281 @@ +// MESSAGE REMOTE_LOG_BLOCK_STATUS PACKING + +#define MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS 185 + +typedef struct __mavlink_remote_log_block_status_t +{ + uint32_t seqno; ///< log data block sequence number + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t status; ///< log data block status +} mavlink_remote_log_block_status_t; + +#define MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN 7 +#define MAVLINK_MSG_ID_185_LEN 7 + +#define MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC 186 +#define MAVLINK_MSG_ID_185_CRC 186 + + + +#define MAVLINK_MESSAGE_INFO_REMOTE_LOG_BLOCK_STATUS { \ + "REMOTE_LOG_BLOCK_STATUS", \ + 4, \ + { { "seqno", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_remote_log_block_status_t, seqno) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_remote_log_block_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_remote_log_block_status_t, target_component) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_remote_log_block_status_t, status) }, \ + } \ +} + + +/** + * @brief Pack a remote_log_block_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param status log data block status + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_remote_log_block_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t seqno, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#else + mavlink_remote_log_block_status_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +} + +/** + * @brief Pack a remote_log_block_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param status log data block status + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_remote_log_block_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t seqno,uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#else + mavlink_remote_log_block_status_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +} + +/** + * @brief Encode a remote_log_block_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param remote_log_block_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_remote_log_block_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_remote_log_block_status_t* remote_log_block_status) +{ + return mavlink_msg_remote_log_block_status_pack(system_id, component_id, msg, remote_log_block_status->target_system, remote_log_block_status->target_component, remote_log_block_status->seqno, remote_log_block_status->status); +} + +/** + * @brief Encode a remote_log_block_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param remote_log_block_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_remote_log_block_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_remote_log_block_status_t* remote_log_block_status) +{ + return mavlink_msg_remote_log_block_status_pack_chan(system_id, component_id, chan, msg, remote_log_block_status->target_system, remote_log_block_status->target_component, remote_log_block_status->seqno, remote_log_block_status->status); +} + +/** + * @brief Send a remote_log_block_status message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param status log data block status + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_remote_log_block_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t seqno, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +#else + mavlink_remote_log_block_status_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, (const char *)&packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, (const char *)&packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_remote_log_block_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t seqno, uint8_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, buf, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +#else + mavlink_remote_log_block_status_t *packet = (mavlink_remote_log_block_status_t *)msgbuf; + packet->seqno = seqno; + packet->target_system = target_system; + packet->target_component = target_component; + packet->status = status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, (const char *)packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS, (const char *)packet, MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE REMOTE_LOG_BLOCK_STATUS UNPACKING + + +/** + * @brief Get field target_system from remote_log_block_status message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_remote_log_block_status_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from remote_log_block_status message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_remote_log_block_status_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field seqno from remote_log_block_status message + * + * @return log data block sequence number + */ +static inline uint32_t mavlink_msg_remote_log_block_status_get_seqno(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field status from remote_log_block_status message + * + * @return log data block status + */ +static inline uint8_t mavlink_msg_remote_log_block_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Decode a remote_log_block_status message into a struct + * + * @param msg The message to decode + * @param remote_log_block_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_remote_log_block_status_decode(const mavlink_message_t* msg, mavlink_remote_log_block_status_t* remote_log_block_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + remote_log_block_status->seqno = mavlink_msg_remote_log_block_status_get_seqno(msg); + remote_log_block_status->target_system = mavlink_msg_remote_log_block_status_get_target_system(msg); + remote_log_block_status->target_component = mavlink_msg_remote_log_block_status_get_target_component(msg); + remote_log_block_status->status = mavlink_msg_remote_log_block_status_get_status(msg); +#else + memcpy(remote_log_block_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_data_block.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_data_block.h new file mode 100644 index 0000000..3aae9e5 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_remote_log_data_block.h @@ -0,0 +1,273 @@ +// MESSAGE REMOTE_LOG_DATA_BLOCK PACKING + +#define MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK 184 + +typedef struct __mavlink_remote_log_data_block_t +{ + uint32_t seqno; ///< log data block sequence number + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t data[200]; ///< log data block +} mavlink_remote_log_data_block_t; + +#define MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN 206 +#define MAVLINK_MSG_ID_184_LEN 206 + +#define MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC 159 +#define MAVLINK_MSG_ID_184_CRC 159 + +#define MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN 200 + +#define MAVLINK_MESSAGE_INFO_REMOTE_LOG_DATA_BLOCK { \ + "REMOTE_LOG_DATA_BLOCK", \ + 4, \ + { { "seqno", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_remote_log_data_block_t, seqno) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_remote_log_data_block_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_remote_log_data_block_t, target_component) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 200, 6, offsetof(mavlink_remote_log_data_block_t, data) }, \ + } \ +} + + +/** + * @brief Pack a remote_log_data_block message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param data log data block + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_remote_log_data_block_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t seqno, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t_array(buf, 6, data, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#else + mavlink_remote_log_data_block_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +} + +/** + * @brief Pack a remote_log_data_block message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param data log data block + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_remote_log_data_block_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t seqno,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t_array(buf, 6, data, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#else + mavlink_remote_log_data_block_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +} + +/** + * @brief Encode a remote_log_data_block struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param remote_log_data_block C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_remote_log_data_block_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_remote_log_data_block_t* remote_log_data_block) +{ + return mavlink_msg_remote_log_data_block_pack(system_id, component_id, msg, remote_log_data_block->target_system, remote_log_data_block->target_component, remote_log_data_block->seqno, remote_log_data_block->data); +} + +/** + * @brief Encode a remote_log_data_block struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param remote_log_data_block C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_remote_log_data_block_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_remote_log_data_block_t* remote_log_data_block) +{ + return mavlink_msg_remote_log_data_block_pack_chan(system_id, component_id, chan, msg, remote_log_data_block->target_system, remote_log_data_block->target_component, remote_log_data_block->seqno, remote_log_data_block->data); +} + +/** + * @brief Send a remote_log_data_block message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seqno log data block sequence number + * @param data log data block + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_remote_log_data_block_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t seqno, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN]; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t_array(buf, 6, data, 200); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +#else + mavlink_remote_log_data_block_t packet; + packet.seqno = seqno; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*200); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, (const char *)&packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, (const char *)&packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_remote_log_data_block_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t seqno, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, seqno); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t_array(buf, 6, data, 200); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, buf, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +#else + mavlink_remote_log_data_block_t *packet = (mavlink_remote_log_data_block_t *)msgbuf; + packet->seqno = seqno; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*200); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, (const char *)packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK, (const char *)packet, MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE REMOTE_LOG_DATA_BLOCK UNPACKING + + +/** + * @brief Get field target_system from remote_log_data_block message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_remote_log_data_block_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from remote_log_data_block message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_remote_log_data_block_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field seqno from remote_log_data_block message + * + * @return log data block sequence number + */ +static inline uint32_t mavlink_msg_remote_log_data_block_get_seqno(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field data from remote_log_data_block message + * + * @return log data block + */ +static inline uint16_t mavlink_msg_remote_log_data_block_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 200, 6); +} + +/** + * @brief Decode a remote_log_data_block message into a struct + * + * @param msg The message to decode + * @param remote_log_data_block C-struct to decode the message contents into + */ +static inline void mavlink_msg_remote_log_data_block_decode(const mavlink_message_t* msg, mavlink_remote_log_data_block_t* remote_log_data_block) +{ +#if MAVLINK_NEED_BYTE_SWAP + remote_log_data_block->seqno = mavlink_msg_remote_log_data_block_get_seqno(msg); + remote_log_data_block->target_system = mavlink_msg_remote_log_data_block_get_target_system(msg); + remote_log_data_block->target_component = mavlink_msg_remote_log_data_block_get_target_component(msg); + mavlink_msg_remote_log_data_block_get_data(msg, remote_log_data_block->data); +#else + memcpy(remote_log_data_block, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_sensor_offsets.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_sensor_offsets.h new file mode 100644 index 0000000..d18f31c --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -0,0 +1,473 @@ +// MESSAGE SENSOR_OFFSETS PACKING + +#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 + +typedef struct __mavlink_sensor_offsets_t +{ + float mag_declination; ///< magnetic declination (radians) + int32_t raw_press; ///< raw pressure from barometer + int32_t raw_temp; ///< raw temperature from barometer + float gyro_cal_x; ///< gyro X calibration + float gyro_cal_y; ///< gyro Y calibration + float gyro_cal_z; ///< gyro Z calibration + float accel_cal_x; ///< accel X calibration + float accel_cal_y; ///< accel Y calibration + float accel_cal_z; ///< accel Z calibration + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset +} mavlink_sensor_offsets_t; + +#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 +#define MAVLINK_MSG_ID_150_LEN 42 + +#define MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC 134 +#define MAVLINK_MSG_ID_150_CRC 134 + + + +#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \ + "SENSOR_OFFSETS", \ + 12, \ + { { "mag_declination", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \ + { "raw_press", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \ + { "raw_temp", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \ + { "gyro_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \ + { "gyro_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \ + { "gyro_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \ + { "accel_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \ + { "accel_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \ + { "accel_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \ + { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \ + } \ +} + + +/** + * @brief Pack a sensor_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +} + +/** + * @brief Pack a sensor_offsets message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +} + +/** + * @brief Encode a sensor_offsets struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + +/** + * @brief Encode a sensor_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + +/** + * @brief Send a sensor_offsets message + * @param chan MAVLink channel to send the message + * + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sensor_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#else + mavlink_sensor_offsets_t *packet = (mavlink_sensor_offsets_t *)msgbuf; + packet->mag_declination = mag_declination; + packet->raw_press = raw_press; + packet->raw_temp = raw_temp; + packet->gyro_cal_x = gyro_cal_x; + packet->gyro_cal_y = gyro_cal_y; + packet->gyro_cal_z = gyro_cal_z; + packet->accel_cal_x = accel_cal_x; + packet->accel_cal_y = accel_cal_y; + packet->accel_cal_z = accel_cal_z; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SENSOR_OFFSETS UNPACKING + + +/** + * @brief Get field mag_ofs_x from sensor_offsets message + * + * @return magnetometer X offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 36); +} + +/** + * @brief Get field mag_ofs_y from sensor_offsets message + * + * @return magnetometer Y offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 38); +} + +/** + * @brief Get field mag_ofs_z from sensor_offsets message + * + * @return magnetometer Z offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field mag_declination from sensor_offsets message + * + * @return magnetic declination (radians) + */ +static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field raw_press from sensor_offsets message + * + * @return raw pressure from barometer + */ +static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field raw_temp from sensor_offsets message + * + * @return raw temperature from barometer + */ +static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field gyro_cal_x from sensor_offsets message + * + * @return gyro X calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field gyro_cal_y from sensor_offsets message + * + * @return gyro Y calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field gyro_cal_z from sensor_offsets message + * + * @return gyro Z calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field accel_cal_x from sensor_offsets message + * + * @return accel X calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field accel_cal_y from sensor_offsets message + * + * @return accel Y calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field accel_cal_z from sensor_offsets message + * + * @return accel Z calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a sensor_offsets message into a struct + * + * @param msg The message to decode + * @param sensor_offsets C-struct to decode the message contents into + */ +static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) +{ +#if MAVLINK_NEED_BYTE_SWAP + sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg); + sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg); + sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg); + sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg); + sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg); + sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg); + sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); + sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); + sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); + sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); + sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); + sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); +#else + memcpy(sensor_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_set_mag_offsets.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_set_mag_offsets.h new file mode 100644 index 0000000..fc6aa71 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -0,0 +1,305 @@ +// MESSAGE SET_MAG_OFFSETS PACKING + +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 + +typedef struct __mavlink_set_mag_offsets_t +{ + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_set_mag_offsets_t; + +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 +#define MAVLINK_MSG_ID_151_LEN 8 + +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC 219 +#define MAVLINK_MSG_ID_151_CRC 219 + + + +#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \ + "SET_MAG_OFFSETS", \ + 5, \ + { { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a set_mag_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +} + +/** + * @brief Pack a set_mag_offsets message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +} + +/** + * @brief Encode a set_mag_offsets struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + +/** + * @brief Encode a set_mag_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + +/** + * @brief Send a set_mag_offsets message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mag_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#else + mavlink_set_mag_offsets_t *packet = (mavlink_set_mag_offsets_t *)msgbuf; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_MAG_OFFSETS UNPACKING + + +/** + * @brief Get field target_system from set_mag_offsets message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field target_component from set_mag_offsets message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field mag_ofs_x from set_mag_offsets message + * + * @return magnetometer X offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field mag_ofs_y from set_mag_offsets message + * + * @return magnetometer Y offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Get field mag_ofs_z from set_mag_offsets message + * + * @return magnetometer Z offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Decode a set_mag_offsets message into a struct + * + * @param msg The message to decode + * @param set_mag_offsets C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); + set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); + set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); + set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); + set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); +#else + memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_simstate.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_simstate.h new file mode 100644 index 0000000..48cfb6f --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_simstate.h @@ -0,0 +1,449 @@ +// MESSAGE SIMSTATE PACKING + +#define MAVLINK_MSG_ID_SIMSTATE 164 + +typedef struct __mavlink_simstate_t +{ + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float xacc; ///< X acceleration m/s/s + float yacc; ///< Y acceleration m/s/s + float zacc; ///< Z acceleration m/s/s + float xgyro; ///< Angular speed around X axis rad/s + float ygyro; ///< Angular speed around Y axis rad/s + float zgyro; ///< Angular speed around Z axis rad/s + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 +} mavlink_simstate_t; + +#define MAVLINK_MSG_ID_SIMSTATE_LEN 44 +#define MAVLINK_MSG_ID_164_LEN 44 + +#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 +#define MAVLINK_MSG_ID_164_CRC 154 + + + +#define MAVLINK_MESSAGE_INFO_SIMSTATE { \ + "SIMSTATE", \ + 11, \ + { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_simstate_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_simstate_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_simstate_t, yaw) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_simstate_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_simstate_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_simstate_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ + } \ +} + + +/** + * @brief Pack a simstate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#else + mavlink_simstate_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SIMSTATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +} + +/** + * @brief Pack a simstate message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#else + mavlink_simstate_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SIMSTATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +} + +/** + * @brief Encode a simstate struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param simstate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_simstate_t* simstate) +{ + return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); +} + +/** + * @brief Encode a simstate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param simstate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) +{ + return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); +} + +/** + * @brief Send a simstate message + * @param chan MAVLink channel to send the message + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#else + mavlink_simstate_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SIMSTATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_simstate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#else + mavlink_simstate_t *packet = (mavlink_simstate_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SIMSTATE UNPACKING + + +/** + * @brief Get field roll from simstate message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_simstate_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pitch from simstate message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_simstate_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from simstate message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_simstate_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field xacc from simstate message + * + * @return X acceleration m/s/s + */ +static inline float mavlink_msg_simstate_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yacc from simstate message + * + * @return Y acceleration m/s/s + */ +static inline float mavlink_msg_simstate_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field zacc from simstate message + * + * @return Z acceleration m/s/s + */ +static inline float mavlink_msg_simstate_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field xgyro from simstate message + * + * @return Angular speed around X axis rad/s + */ +static inline float mavlink_msg_simstate_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field ygyro from simstate message + * + * @return Angular speed around Y axis rad/s + */ +static inline float mavlink_msg_simstate_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field zgyro from simstate message + * + * @return Angular speed around Z axis rad/s + */ +static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field lat from simstate message + * + * @return Latitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field lng from simstate message + * + * @return Longitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 40); +} + +/** + * @brief Decode a simstate message into a struct + * + * @param msg The message to decode + * @param simstate C-struct to decode the message contents into + */ +static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mavlink_simstate_t* simstate) +{ +#if MAVLINK_NEED_BYTE_SWAP + simstate->roll = mavlink_msg_simstate_get_roll(msg); + simstate->pitch = mavlink_msg_simstate_get_pitch(msg); + simstate->yaw = mavlink_msg_simstate_get_yaw(msg); + simstate->xacc = mavlink_msg_simstate_get_xacc(msg); + simstate->yacc = mavlink_msg_simstate_get_yacc(msg); + simstate->zacc = mavlink_msg_simstate_get_zacc(msg); + simstate->xgyro = mavlink_msg_simstate_get_xgyro(msg); + simstate->ygyro = mavlink_msg_simstate_get_ygyro(msg); + simstate->zgyro = mavlink_msg_simstate_get_zgyro(msg); + simstate->lat = mavlink_msg_simstate_get_lat(msg); + simstate->lng = mavlink_msg_simstate_get_lng(msg); +#else + memcpy(simstate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_wind.h b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_wind.h new file mode 100644 index 0000000..5d5edc4 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_wind.h @@ -0,0 +1,257 @@ +// MESSAGE WIND PACKING + +#define MAVLINK_MSG_ID_WIND 168 + +typedef struct __mavlink_wind_t +{ + float direction; ///< wind direction that wind is coming from (degrees) + float speed; ///< wind speed in ground plane (m/s) + float speed_z; ///< vertical wind speed (m/s) +} mavlink_wind_t; + +#define MAVLINK_MSG_ID_WIND_LEN 12 +#define MAVLINK_MSG_ID_168_LEN 12 + +#define MAVLINK_MSG_ID_WIND_CRC 1 +#define MAVLINK_MSG_ID_168_CRC 1 + + + +#define MAVLINK_MESSAGE_INFO_WIND { \ + "WIND", \ + 3, \ + { { "direction", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_wind_t, direction) }, \ + { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_wind_t, speed) }, \ + { "speed_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_wind_t, speed_z) }, \ + } \ +} + + +/** + * @brief Pack a wind message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param direction wind direction that wind is coming from (degrees) + * @param speed wind speed in ground plane (m/s) + * @param speed_z vertical wind speed (m/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float direction, float speed, float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIND_LEN]; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); +#else + mavlink_wind_t packet; + packet.direction = direction; + packet.speed = speed; + packet.speed_z = speed_z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIND; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN); +#endif +} + +/** + * @brief Pack a wind message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param direction wind direction that wind is coming from (degrees) + * @param speed wind speed in ground plane (m/s) + * @param speed_z vertical wind speed (m/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float direction,float speed,float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIND_LEN]; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); +#else + mavlink_wind_t packet; + packet.direction = direction; + packet.speed = speed; + packet.speed_z = speed_z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIND; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN); +#endif +} + +/** + * @brief Encode a wind struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wind_t* wind) +{ + return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z); +} + +/** + * @brief Encode a wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) +{ + return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); +} + +/** + * @brief Send a wind message + * @param chan MAVLink channel to send the message + * + * @param direction wind direction that wind is coming from (degrees) + * @param speed wind speed in ground plane (m/s) + * @param speed_z vertical wind speed (m/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction, float speed, float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIND_LEN]; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); +#endif +#else + mavlink_wind_t packet; + packet.direction = direction; + packet.speed = speed; + packet.speed_z = speed_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float direction, float speed, float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); +#endif +#else + mavlink_wind_t *packet = (mavlink_wind_t *)msgbuf; + packet->direction = direction; + packet->speed = speed; + packet->speed_z = speed_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE WIND UNPACKING + + +/** + * @brief Get field direction from wind message + * + * @return wind direction that wind is coming from (degrees) + */ +static inline float mavlink_msg_wind_get_direction(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field speed from wind message + * + * @return wind speed in ground plane (m/s) + */ +static inline float mavlink_msg_wind_get_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field speed_z from wind message + * + * @return vertical wind speed (m/s) + */ +static inline float mavlink_msg_wind_get_speed_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a wind message into a struct + * + * @param msg The message to decode + * @param wind C-struct to decode the message contents into + */ +static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink_wind_t* wind) +{ +#if MAVLINK_NEED_BYTE_SWAP + wind->direction = mavlink_msg_wind_get_direction(msg); + wind->speed = mavlink_msg_wind_get_speed(msg); + wind->speed_z = mavlink_msg_wind_get_speed_z(msg); +#else + memcpy(wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WIND_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/ardupilotmega/testsuite.h b/flightcode/mavlink/c_library/ardupilotmega/testsuite.h new file mode 100644 index 0000000..4690b31 --- /dev/null +++ b/flightcode/mavlink/c_library/ardupilotmega/testsuite.h @@ -0,0 +1,2437 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef ARDUPILOTMEGA_TESTSUITE_H +#define ARDUPILOTMEGA_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_ardupilotmega(system_id, component_id, last_msg); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_sensor_offsets_t packet_in = { + 17.0,963497672,963497880,101.0,129.0,157.0,185.0,213.0,241.0,19107,19211,19315 + }; + mavlink_sensor_offsets_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mag_declination = packet_in.mag_declination; + packet1.raw_press = packet_in.raw_press; + packet1.raw_temp = packet_in.raw_temp; + packet1.gyro_cal_x = packet_in.gyro_cal_x; + packet1.gyro_cal_y = packet_in.gyro_cal_y; + packet1.gyro_cal_z = packet_in.gyro_cal_z; + packet1.accel_cal_x = packet_in.accel_cal_x; + packet1.accel_cal_y = packet_in.accel_cal_y; + packet1.accel_cal_z = packet_in.accel_cal_z; + packet1.mag_ofs_x = packet_in.mag_ofs_x; + packet1.mag_ofs_y = packet_in.mag_ofs_y; + packet1.mag_ofs_z = packet_in.mag_ofs_z; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +#ifndef HAVE_CRC_ACCUMULATE +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp = data ^ (uint8_t)(*crcAccum &0xff); + tmp ^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +} +#endif + + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) +{ + uint16_t crcTmp; + crc_init(&crcTmp); + while (length--) { + crc_accumulate(*pBuffer++, &crcTmp); + } + return crcTmp; +} + + +/** + * @brief Accumulate the X.25 CRC by adding an array of bytes + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new bytes to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) +{ + const uint8_t *p = (const uint8_t *)pBuffer; + while (length--) { + crc_accumulate(*p++, crcAccum); + } +} + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/flightcode/mavlink/c_library/common/common.h b/flightcode/mavlink/c_library/common/common.h new file mode 100644 index 0000000..e6ba875 --- /dev/null +++ b/flightcode/mavlink/c_library/common/common.h @@ -0,0 +1,757 @@ +/** @file + * @brief MAVLink comm protocol generated from common.xml + * @see http://mavlink.org + */ +#ifndef MAVLINK_COMMON_H +#define MAVLINK_COMMON_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 229, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 12, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 0, 0, 0, 0, 0, 0, 36, 60, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 254, 36, 30, 18, 18, 51, 9, 0} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 59, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 0, 0, 0, 0, 0, 0, 154, 178, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 204, 49, 170, 44, 83, 46, 0} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_COMMON + +// ENUM DEFINITIONS + + +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +#ifndef HAVE_ENUM_MAV_AUTOPILOT +#define HAVE_ENUM_MAV_AUTOPILOT +typedef enum MAV_AUTOPILOT +{ + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://pixhawk.ethz.ch/px4/ | */ + MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ + MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ + MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ + MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ + MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ + MAV_AUTOPILOT_ENUM_END=18, /* | */ +} MAV_AUTOPILOT; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_TYPE +#define HAVE_ENUM_MAV_TYPE +typedef enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Octorotor | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_KITE=17, /* Flapping wing | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ + MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ + MAV_TYPE_VTOL_RESERVED1=21, /* VTOL reserved 1 | */ + MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ + MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ + MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ + MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ + MAV_TYPE_GIMBAL=26, /* Onboard gimbal | */ + MAV_TYPE_ENUM_END=27, /* | */ +} MAV_TYPE; +#endif + +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +typedef enum MAV_MODE_FLAG +{ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END=129, /* | */ +} MAV_MODE_FLAG; +#endif + +/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +typedef enum MAV_MODE_FLAG_DECODE_POSITION +{ + MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ + MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ + MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit: 00000100 | */ + MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ + MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ + MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ + MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ +} MAV_MODE_FLAG_DECODE_POSITION; +#endif + +/** @brief Override command, pauses current mission execution and moves immediately to a position */ +#ifndef HAVE_ENUM_MAV_GOTO +#define HAVE_ENUM_MAV_GOTO +typedef enum MAV_GOTO +{ + MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */ + MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */ + MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */ + MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */ + MAV_GOTO_ENUM_END=4, /* | */ +} MAV_GOTO; +#endif + +/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it + simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */ +#ifndef HAVE_ENUM_MAV_MODE +#define HAVE_ENUM_MAV_MODE +typedef enum MAV_MODE +{ + MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */ + MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */ + MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ + MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */ + MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ + MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */ + MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ + MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */ + MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ + MAV_MODE_ENUM_END=221, /* | */ +} MAV_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_STATE +#define HAVE_ENUM_MAV_STATE +typedef enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_ENUM_END=8, /* | */ +} MAV_STATE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_COMPONENT +#define HAVE_ENUM_MAV_COMPONENT +typedef enum MAV_COMPONENT +{ + MAV_COMP_ID_ALL=0, /* | */ + MAV_COMP_ID_CAMERA=100, /* | */ + MAV_COMP_ID_SERVO1=140, /* | */ + MAV_COMP_ID_SERVO2=141, /* | */ + MAV_COMP_ID_SERVO3=142, /* | */ + MAV_COMP_ID_SERVO4=143, /* | */ + MAV_COMP_ID_SERVO5=144, /* | */ + MAV_COMP_ID_SERVO6=145, /* | */ + MAV_COMP_ID_SERVO7=146, /* | */ + MAV_COMP_ID_SERVO8=147, /* | */ + MAV_COMP_ID_SERVO9=148, /* | */ + MAV_COMP_ID_SERVO10=149, /* | */ + MAV_COMP_ID_SERVO11=150, /* | */ + MAV_COMP_ID_SERVO12=151, /* | */ + MAV_COMP_ID_SERVO13=152, /* | */ + MAV_COMP_ID_SERVO14=153, /* | */ + MAV_COMP_ID_GIMBAL=154, /* | */ + MAV_COMP_ID_LOG=155, /* | */ + MAV_COMP_ID_MAPPER=180, /* | */ + MAV_COMP_ID_MISSIONPLANNER=190, /* | */ + MAV_COMP_ID_PATHPLANNER=195, /* | */ + MAV_COMP_ID_IMU=200, /* | */ + MAV_COMP_ID_IMU_2=201, /* | */ + MAV_COMP_ID_IMU_3=202, /* | */ + MAV_COMP_ID_GPS=220, /* | */ + MAV_COMP_ID_UDP_BRIDGE=240, /* | */ + MAV_COMP_ID_UART_BRIDGE=241, /* | */ + MAV_COMP_ID_SYSTEM_CONTROL=250, /* | */ + MAV_COMPONENT_ENUM_END=251, /* | */ +} MAV_COMPONENT; +#endif + +/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ +#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR +#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR +typedef enum MAV_SYS_STATUS_SENSOR +{ + MAV_SYS_STATUS_SENSOR_3D_GYRO=1, /* 0x01 3D gyro | */ + MAV_SYS_STATUS_SENSOR_3D_ACCEL=2, /* 0x02 3D accelerometer | */ + MAV_SYS_STATUS_SENSOR_3D_MAG=4, /* 0x04 3D magnetometer | */ + MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE=8, /* 0x08 absolute pressure | */ + MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE=16, /* 0x10 differential pressure | */ + MAV_SYS_STATUS_SENSOR_GPS=32, /* 0x20 GPS | */ + MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW=64, /* 0x40 optical flow | */ + MAV_SYS_STATUS_SENSOR_VISION_POSITION=128, /* 0x80 computer vision position | */ + MAV_SYS_STATUS_SENSOR_LASER_POSITION=256, /* 0x100 laser based position | */ + MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH=512, /* 0x200 external ground truth (Vicon or Leica) | */ + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL=1024, /* 0x400 3D angular rate control | */ + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION=2048, /* 0x800 attitude stabilization | */ + MAV_SYS_STATUS_SENSOR_YAW_POSITION=4096, /* 0x1000 yaw position | */ + MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */ + MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */ + MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */ + MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */ + MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ + MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ + MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ + MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ + MAV_SYS_STATUS_AHRS=2097152, /* 0x200000 AHRS subsystem health | */ + MAV_SYS_STATUS_TERRAIN=4194304, /* 0x400000 Terrain subsystem health | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=4194305, /* | */ +} MAV_SYS_STATUS_SENSOR; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_FRAME +#define HAVE_ENUM_MAV_FRAME +typedef enum MAV_FRAME +{ + MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */ + MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */ + MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ + MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. | */ + MAV_FRAME_LOCAL_OFFSET_NED=7, /* Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | */ + MAV_FRAME_BODY_NED=8, /* Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | */ + MAV_FRAME_BODY_OFFSET_NED=9, /* Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT_INT=11, /* Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ + MAV_FRAME_ENUM_END=12, /* | */ +} MAV_FRAME; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE +#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE +typedef enum MAVLINK_DATA_STREAM_TYPE +{ + MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ + MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ + MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ + MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ + MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ +} MAVLINK_DATA_STREAM_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_ACTION +#define HAVE_ENUM_FENCE_ACTION +typedef enum FENCE_ACTION +{ + FENCE_ACTION_NONE=0, /* Disable fenced mode | */ + FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ + FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ + FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ + FENCE_ACTION_ENUM_END=4, /* | */ +} FENCE_ACTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_BREACH +#define HAVE_ENUM_FENCE_BREACH +typedef enum FENCE_BREACH +{ + FENCE_BREACH_NONE=0, /* No last fence breach | */ + FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ + FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ + FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ + FENCE_BREACH_ENUM_END=4, /* | */ +} FENCE_BREACH; +#endif + +/** @brief Enumeration of possible mount operation modes */ +#ifndef HAVE_ENUM_MAV_MOUNT_MODE +#define HAVE_ENUM_MAV_MOUNT_MODE +typedef enum MAV_MOUNT_MODE +{ + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ +} MAV_MOUNT_MODE; +#endif + +/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. */ +#ifndef HAVE_ENUM_MAV_DATA_STREAM +#define HAVE_ENUM_MAV_DATA_STREAM +typedef enum MAV_DATA_STREAM +{ + MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END=13, /* | */ +} MAV_DATA_STREAM; +#endif + +/** @brief The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). */ +#ifndef HAVE_ENUM_MAV_ROI +#define HAVE_ENUM_MAV_ROI +typedef enum MAV_ROI +{ + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */ + MAV_ROI_WPINDEX=2, /* Point toward given MISSION. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ +} MAV_ROI; +#endif + +/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ +#ifndef HAVE_ENUM_MAV_CMD_ACK +#define HAVE_ENUM_MAV_CMD_ACK +typedef enum MAV_CMD_ACK +{ + MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ + MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */ + MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. | */ + MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. | */ + MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. | */ + MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. | */ + MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. | */ + MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */ + MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */ + MAV_CMD_ACK_ENUM_END=10, /* | */ +} MAV_CMD_ACK; +#endif + +/** @brief Specifies the datatype of a MAVLink parameter. */ +#ifndef HAVE_ENUM_MAV_PARAM_TYPE +#define HAVE_ENUM_MAV_PARAM_TYPE +typedef enum MAV_PARAM_TYPE +{ + MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ + MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ + MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ + MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ + MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ + MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ + MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ + MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ + MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ + MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ + MAV_PARAM_TYPE_ENUM_END=11, /* | */ +} MAV_PARAM_TYPE; +#endif + +/** @brief result from a mavlink command */ +#ifndef HAVE_ENUM_MAV_RESULT +#define HAVE_ENUM_MAV_RESULT +typedef enum MAV_RESULT +{ + MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ + MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ + MAV_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ + MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ + MAV_RESULT_FAILED=4, /* Command executed, but failed | */ + MAV_RESULT_ENUM_END=5, /* | */ +} MAV_RESULT; +#endif + +/** @brief result in a mavlink mission ack */ +#ifndef HAVE_ENUM_MAV_MISSION_RESULT +#define HAVE_ENUM_MAV_MISSION_RESULT +typedef enum MAV_MISSION_RESULT +{ + MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ + MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */ + MAV_MISSION_UNSUPPORTED_FRAME=2, /* coordinate frame is not supported | */ + MAV_MISSION_UNSUPPORTED=3, /* command is not supported | */ + MAV_MISSION_NO_SPACE=4, /* mission item exceeds storage space | */ + MAV_MISSION_INVALID=5, /* one of the parameters has an invalid value | */ + MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value | */ + MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value | */ + MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value | */ + MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value | */ + MAV_MISSION_INVALID_PARAM5_X=10, /* x/param5 has an invalid value | */ + MAV_MISSION_INVALID_PARAM6_Y=11, /* y/param6 has an invalid value | */ + MAV_MISSION_INVALID_PARAM7=12, /* param7 has an invalid value | */ + MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */ + MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ + MAV_MISSION_RESULT_ENUM_END=15, /* | */ +} MAV_MISSION_RESULT; +#endif + +/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ +#ifndef HAVE_ENUM_MAV_SEVERITY +#define HAVE_ENUM_MAV_SEVERITY +typedef enum MAV_SEVERITY +{ + MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ + MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ + MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ + MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ + MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ + MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */ + MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ + MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ + MAV_SEVERITY_ENUM_END=8, /* | */ +} MAV_SEVERITY; +#endif + +/** @brief Power supply status flags (bitmask) */ +#ifndef HAVE_ENUM_MAV_POWER_STATUS +#define HAVE_ENUM_MAV_POWER_STATUS +typedef enum MAV_POWER_STATUS +{ + MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ + MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ + MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ + MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ + MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ + MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ + MAV_POWER_STATUS_ENUM_END=33, /* | */ +} MAV_POWER_STATUS; +#endif + +/** @brief SERIAL_CONTROL device types */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV +#define HAVE_ENUM_SERIAL_CONTROL_DEV +typedef enum SERIAL_CONTROL_DEV +{ + SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ + SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ + SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ + SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ + SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ +} SERIAL_CONTROL_DEV; +#endif + +/** @brief SERIAL_CONTROL flags (bitmask) */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG +#define HAVE_ENUM_SERIAL_CONTROL_FLAG +typedef enum SERIAL_CONTROL_FLAG +{ + SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ + SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ + SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ + SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ + SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ + SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ +} SERIAL_CONTROL_FLAG; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +typedef enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Ultrasound rangefinder, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_INFRARED=2, /* Infrared rangefinder, e.g. Sharp units | */ + MAV_DISTANCE_SENSOR_ENUM_END=3, /* | */ +} MAV_DISTANCE_SENSOR; +#endif + +/** @brief Enumeration of sensor orientation, according to its rotations */ +#ifndef HAVE_ENUM_MAV_SENSOR_ORIENTATION +#define HAVE_ENUM_MAV_SENSOR_ORIENTATION +typedef enum MAV_SENSOR_ORIENTATION +{ + MAV_SENSOR_ROTATION_NONE=0, /* Roll: 0, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_YAW_45=1, /* Roll: 0, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_YAW_90=2, /* Roll: 0, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_YAW_135=3, /* Roll: 0, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_YAW_180=4, /* Roll: 0, Pitch: 0, Yaw: 180 | */ + MAV_SENSOR_ROTATION_YAW_225=5, /* Roll: 0, Pitch: 0, Yaw: 225 | */ + MAV_SENSOR_ROTATION_YAW_270=6, /* Roll: 0, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_YAW_315=7, /* Roll: 0, Pitch: 0, Yaw: 315 | */ + MAV_SENSOR_ROTATION_ROLL_180=8, /* Roll: 180, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_45=9, /* Roll: 180, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_90=10, /* Roll: 180, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_135=11, /* Roll: 180, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_PITCH_180=12, /* Roll: 0, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_225=13, /* Roll: 180, Pitch: 0, Yaw: 225 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_270=14, /* Roll: 180, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_180_YAW_315=15, /* Roll: 180, Pitch: 0, Yaw: 315 | */ + MAV_SENSOR_ROTATION_ROLL_90=16, /* Roll: 90, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_45=17, /* Roll: 90, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_90=18, /* Roll: 90, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_135=19, /* Roll: 90, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_ROLL_270=20, /* Roll: 270, Pitch: 0, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_45=21, /* Roll: 270, Pitch: 0, Yaw: 45 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_90=22, /* Roll: 270, Pitch: 0, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_270_YAW_135=23, /* Roll: 270, Pitch: 0, Yaw: 135 | */ + MAV_SENSOR_ROTATION_PITCH_90=24, /* Roll: 0, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_PITCH_270=25, /* Roll: 0, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_PITCH_180_YAW_90=26, /* Roll: 0, Pitch: 180, Yaw: 90 | */ + MAV_SENSOR_ROTATION_PITCH_180_YAW_270=27, /* Roll: 0, Pitch: 180, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_90=28, /* Roll: 90, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_PITCH_90=29, /* Roll: 180, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_90=30, /* Roll: 270, Pitch: 90, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_180=31, /* Roll: 90, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_180=32, /* Roll: 270, Pitch: 180, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_270=33, /* Roll: 90, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_180_PITCH_270=34, /* Roll: 180, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_270_PITCH_270=35, /* Roll: 270, Pitch: 270, Yaw: 0 | */ + MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90=36, /* Roll: 90, Pitch: 180, Yaw: 90 | */ + MAV_SENSOR_ROTATION_ROLL_90_YAW_270=37, /* Roll: 90, Pitch: 0, Yaw: 270 | */ + MAV_SENSOR_ROTATION_ROLL_315_PITCH_315_YAW_315=38, /* Roll: 315, Pitch: 315, Yaw: 315 | */ + MAV_SENSOR_ORIENTATION_ENUM_END=39, /* | */ +} MAV_SENSOR_ORIENTATION; +#endif + +/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ +#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +typedef enum MAV_PROTOCOL_CAPABILITY +{ + MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports MISSION float message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_UNION=16, /* Autopilot supports the new param union message type. | */ + MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the new param union message type. | */ + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ + MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ + MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET=1024, /* Autopilot supports direct actuator control. | */ + MAV_PROTOCOL_CAPABILITY_ENUM_END=1025, /* | */ +} MAV_PROTOCOL_CAPABILITY; +#endif + +/** @brief Enumeration of estimator types */ +#ifndef HAVE_ENUM_MAV_ESTIMATOR_TYPE +#define HAVE_ENUM_MAV_ESTIMATOR_TYPE +typedef enum MAV_ESTIMATOR_TYPE +{ + MAV_ESTIMATOR_TYPE_NAIVE=1, /* This is a naive estimator without any real covariance feedback. | */ + MAV_ESTIMATOR_TYPE_VISION=2, /* Computer vision based estimate. Might be up to scale. | */ + MAV_ESTIMATOR_TYPE_VIO=3, /* Visual-inertial estimate. | */ + MAV_ESTIMATOR_TYPE_GPS=4, /* Plain GPS estimate. | */ + MAV_ESTIMATOR_TYPE_GPS_INS=5, /* Estimator integrating GPS and inertial sensing. | */ + MAV_ESTIMATOR_TYPE_ENUM_END=6, /* | */ +} MAV_ESTIMATOR_TYPE; +#endif + +/** @brief Enumeration of battery types */ +#ifndef HAVE_ENUM_MAV_BATTERY_TYPE +#define HAVE_ENUM_MAV_BATTERY_TYPE +typedef enum MAV_BATTERY_TYPE +{ + MAV_BATTERY_TYPE_UNKNOWN=0, /* Not specified. | */ + MAV_BATTERY_TYPE_LIPO=1, /* Lithium polymere battery | */ + MAV_BATTERY_TYPE_LIFE=2, /* Lithium ferrite battery | */ + MAV_BATTERY_TYPE_LION=3, /* Lithium-ION battery | */ + MAV_BATTERY_TYPE_NIMH=4, /* Nickel metal hydride battery | */ + MAV_BATTERY_TYPE_ENUM_END=5, /* | */ +} MAV_BATTERY_TYPE; +#endif + +/** @brief Enumeration of battery functions */ +#ifndef HAVE_ENUM_MAV_BATTERY_FUNCTION +#define HAVE_ENUM_MAV_BATTERY_FUNCTION +typedef enum MAV_BATTERY_FUNCTION +{ + MAV_BATTERY_FUNCTION_UNKNOWN=0, /* Lithium polymere battery | */ + MAV_BATTERY_FUNCTION_ALL=1, /* Battery supports all flight systems | */ + MAV_BATTERY_FUNCTION_PROPULSION=2, /* Battery for the propulsion system | */ + MAV_BATTERY_FUNCTION_AVIONICS=3, /* Avionics battery | */ + MAV_BATTERY_TYPE_PAYLOAD=4, /* Payload battery | */ + MAV_BATTERY_FUNCTION_ENUM_END=5, /* | */ +} MAV_BATTERY_FUNCTION; +#endif + + + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_heartbeat.h" +#include "./mavlink_msg_sys_status.h" +#include "./mavlink_msg_system_time.h" +#include "./mavlink_msg_ping.h" +#include "./mavlink_msg_change_operator_control.h" +#include "./mavlink_msg_change_operator_control_ack.h" +#include "./mavlink_msg_auth_key.h" +#include "./mavlink_msg_set_mode.h" +#include "./mavlink_msg_param_request_read.h" +#include "./mavlink_msg_param_request_list.h" +#include "./mavlink_msg_param_value.h" +#include "./mavlink_msg_param_set.h" +#include "./mavlink_msg_gps_raw_int.h" +#include "./mavlink_msg_gps_status.h" +#include "./mavlink_msg_scaled_imu.h" +#include "./mavlink_msg_raw_imu.h" +#include "./mavlink_msg_raw_pressure.h" +#include "./mavlink_msg_scaled_pressure.h" +#include "./mavlink_msg_attitude.h" +#include "./mavlink_msg_attitude_quaternion.h" +#include "./mavlink_msg_local_position_ned.h" +#include "./mavlink_msg_global_position_int.h" +#include "./mavlink_msg_rc_channels_scaled.h" +#include "./mavlink_msg_rc_channels_raw.h" +#include "./mavlink_msg_servo_output_raw.h" +#include "./mavlink_msg_mission_request_partial_list.h" +#include "./mavlink_msg_mission_write_partial_list.h" +#include "./mavlink_msg_mission_item.h" +#include "./mavlink_msg_mission_request.h" +#include "./mavlink_msg_mission_set_current.h" +#include "./mavlink_msg_mission_current.h" +#include "./mavlink_msg_mission_request_list.h" +#include "./mavlink_msg_mission_count.h" +#include "./mavlink_msg_mission_clear_all.h" +#include "./mavlink_msg_mission_item_reached.h" +#include "./mavlink_msg_mission_ack.h" +#include "./mavlink_msg_set_gps_global_origin.h" +#include "./mavlink_msg_gps_global_origin.h" +#include "./mavlink_msg_param_map_rc.h" +#include "./mavlink_msg_safety_set_allowed_area.h" +#include "./mavlink_msg_safety_allowed_area.h" +#include "./mavlink_msg_attitude_quaternion_cov.h" +#include "./mavlink_msg_nav_controller_output.h" +#include "./mavlink_msg_global_position_int_cov.h" +#include "./mavlink_msg_local_position_ned_cov.h" +#include "./mavlink_msg_rc_channels.h" +#include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_data_stream.h" +#include "./mavlink_msg_manual_control.h" +#include "./mavlink_msg_rc_channels_override.h" +#include "./mavlink_msg_mission_item_int.h" +#include "./mavlink_msg_vfr_hud.h" +#include "./mavlink_msg_command_int.h" +#include "./mavlink_msg_command_long.h" +#include "./mavlink_msg_command_ack.h" +#include "./mavlink_msg_manual_setpoint.h" +#include "./mavlink_msg_set_attitude_target.h" +#include "./mavlink_msg_attitude_target.h" +#include "./mavlink_msg_set_position_target_local_ned.h" +#include "./mavlink_msg_position_target_local_ned.h" +#include "./mavlink_msg_set_position_target_global_int.h" +#include "./mavlink_msg_position_target_global_int.h" +#include "./mavlink_msg_local_position_ned_system_global_offset.h" +#include "./mavlink_msg_hil_state.h" +#include "./mavlink_msg_hil_controls.h" +#include "./mavlink_msg_hil_rc_inputs_raw.h" +#include "./mavlink_msg_optical_flow.h" +#include "./mavlink_msg_global_vision_position_estimate.h" +#include "./mavlink_msg_vision_position_estimate.h" +#include "./mavlink_msg_vision_speed_estimate.h" +#include "./mavlink_msg_vicon_position_estimate.h" +#include "./mavlink_msg_highres_imu.h" +#include "./mavlink_msg_optical_flow_rad.h" +#include "./mavlink_msg_hil_sensor.h" +#include "./mavlink_msg_sim_state.h" +#include "./mavlink_msg_radio_status.h" +#include "./mavlink_msg_file_transfer_protocol.h" +#include "./mavlink_msg_timesync.h" +#include "./mavlink_msg_camera_trigger.h" +#include "./mavlink_msg_hil_gps.h" +#include "./mavlink_msg_hil_optical_flow.h" +#include "./mavlink_msg_hil_state_quaternion.h" +#include "./mavlink_msg_scaled_imu2.h" +#include "./mavlink_msg_log_request_list.h" +#include "./mavlink_msg_log_entry.h" +#include "./mavlink_msg_log_request_data.h" +#include "./mavlink_msg_log_data.h" +#include "./mavlink_msg_log_erase.h" +#include "./mavlink_msg_log_request_end.h" +#include "./mavlink_msg_gps_inject_data.h" +#include "./mavlink_msg_gps2_raw.h" +#include "./mavlink_msg_power_status.h" +#include "./mavlink_msg_serial_control.h" +#include "./mavlink_msg_gps_rtk.h" +#include "./mavlink_msg_gps2_rtk.h" +#include "./mavlink_msg_scaled_imu3.h" +#include "./mavlink_msg_data_transmission_handshake.h" +#include "./mavlink_msg_encapsulated_data.h" +#include "./mavlink_msg_distance_sensor.h" +#include "./mavlink_msg_terrain_request.h" +#include "./mavlink_msg_terrain_data.h" +#include "./mavlink_msg_terrain_check.h" +#include "./mavlink_msg_terrain_report.h" +#include "./mavlink_msg_scaled_pressure2.h" +#include "./mavlink_msg_att_pos_mocap.h" +#include "./mavlink_msg_set_actuator_control_target.h" +#include "./mavlink_msg_actuator_control_target.h" +#include "./mavlink_msg_battery_status.h" +#include "./mavlink_msg_autopilot_version.h" +#include "./mavlink_msg_landing_target.h" +#include "./mavlink_msg_v2_extension.h" +#include "./mavlink_msg_memory_vect.h" +#include "./mavlink_msg_debug_vect.h" +#include "./mavlink_msg_named_value_float.h" +#include "./mavlink_msg_named_value_int.h" +#include "./mavlink_msg_statustext.h" +#include "./mavlink_msg_debug.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_COMMON_H diff --git a/flightcode/mavlink/c_library/common/mavlink.h b/flightcode/mavlink/c_library/common/mavlink.h new file mode 100644 index 0000000..f92b2b2 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink.h @@ -0,0 +1,27 @@ +/** @file + * @brief MAVLink comm protocol built from common.xml + * @see http://mavlink.org + */ +#ifndef MAVLINK_H +#define MAVLINK_H + +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#include "version.h" +#include "common.h" + +#endif // MAVLINK_H diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_actuator_control_target.h b/flightcode/mavlink/c_library/common/mavlink_msg_actuator_control_target.h new file mode 100644 index 0000000..9c33833 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_actuator_control_target.h @@ -0,0 +1,249 @@ +// MESSAGE ACTUATOR_CONTROL_TARGET PACKING + +#define MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET 140 + +typedef struct __mavlink_actuator_control_target_t +{ + uint64_t time_usec; ///< Timestamp (micros since boot or Unix epoch) + float controls[8]; ///< Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + uint8_t group_mlx; ///< Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. +} mavlink_actuator_control_target_t; + +#define MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN 41 +#define MAVLINK_MSG_ID_140_LEN 41 + +#define MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC 181 +#define MAVLINK_MSG_ID_140_CRC 181 + +#define MAVLINK_MSG_ACTUATOR_CONTROL_TARGET_FIELD_CONTROLS_LEN 8 + +#define MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET { \ + "ACTUATOR_CONTROL_TARGET", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_actuator_control_target_t, time_usec) }, \ + { "controls", NULL, MAVLINK_TYPE_FLOAT, 8, 8, offsetof(mavlink_actuator_control_target_t, controls) }, \ + { "group_mlx", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_actuator_control_target_t, group_mlx) }, \ + } \ +} + + +/** + * @brief Pack a actuator_control_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_control_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t group_mlx, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + +/** + * @brief Pack a actuator_control_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_control_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t group_mlx,const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + +/** + * @brief Encode a actuator_control_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_control_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_actuator_control_target_t* actuator_control_target) +{ + return mavlink_msg_actuator_control_target_pack(system_id, component_id, msg, actuator_control_target->time_usec, actuator_control_target->group_mlx, actuator_control_target->controls); +} + +/** + * @brief Encode a actuator_control_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_control_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_actuator_control_target_t* actuator_control_target) +{ + return mavlink_msg_actuator_control_target_pack_chan(system_id, component_id, chan, msg, actuator_control_target->time_usec, actuator_control_target->group_mlx, actuator_control_target->controls); +} + +/** + * @brief Send a actuator_control_target message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_actuator_control_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t group_mlx, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_float_array(buf, 8, controls, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#else + mavlink_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_actuator_control_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t group_mlx, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_float_array(buf, 8, controls, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#else + mavlink_actuator_control_target_t *packet = (mavlink_actuator_control_target_t *)msgbuf; + packet->time_usec = time_usec; + packet->group_mlx = group_mlx; + mav_array_memcpy(packet->controls, controls, sizeof(float)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ACTUATOR_CONTROL_TARGET UNPACKING + + +/** + * @brief Get field time_usec from actuator_control_target message + * + * @return Timestamp (micros since boot or Unix epoch) + */ +static inline uint64_t mavlink_msg_actuator_control_target_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field group_mlx from actuator_control_target message + * + * @return Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + */ +static inline uint8_t mavlink_msg_actuator_control_target_get_group_mlx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field controls from actuator_control_target message + * + * @return Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + */ +static inline uint16_t mavlink_msg_actuator_control_target_get_controls(const mavlink_message_t* msg, float *controls) +{ + return _MAV_RETURN_float_array(msg, controls, 8, 8); +} + +/** + * @brief Decode a actuator_control_target message into a struct + * + * @param msg The message to decode + * @param actuator_control_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_actuator_control_target_decode(const mavlink_message_t* msg, mavlink_actuator_control_target_t* actuator_control_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + actuator_control_target->time_usec = mavlink_msg_actuator_control_target_get_time_usec(msg); + mavlink_msg_actuator_control_target_get_controls(msg, actuator_control_target->controls); + actuator_control_target->group_mlx = mavlink_msg_actuator_control_target_get_group_mlx(msg); +#else + memcpy(actuator_control_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_att_pos_mocap.h b/flightcode/mavlink/c_library/common/mavlink_msg_att_pos_mocap.h new file mode 100644 index 0000000..f776883 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_att_pos_mocap.h @@ -0,0 +1,297 @@ +// MESSAGE ATT_POS_MOCAP PACKING + +#define MAVLINK_MSG_ID_ATT_POS_MOCAP 138 + +typedef struct __mavlink_att_pos_mocap_t +{ + uint64_t time_usec; ///< Timestamp (micros since boot or Unix epoch) + float q[4]; ///< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float x; ///< X position in meters (NED) + float y; ///< Y position in meters (NED) + float z; ///< Z position in meters (NED) +} mavlink_att_pos_mocap_t; + +#define MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN 36 +#define MAVLINK_MSG_ID_138_LEN 36 + +#define MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC 109 +#define MAVLINK_MSG_ID_138_CRC 109 + +#define MAVLINK_MSG_ATT_POS_MOCAP_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP { \ + "ATT_POS_MOCAP", \ + 5, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_att_pos_mocap_t, time_usec) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_att_pos_mocap_t, q) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_att_pos_mocap_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_att_pos_mocap_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_att_pos_mocap_t, z) }, \ + } \ +} + + +/** + * @brief Pack a att_pos_mocap message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param x X position in meters (NED) + * @param y Y position in meters (NED) + * @param z Z position in meters (NED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, const float *q, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#else + mavlink_att_pos_mocap_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATT_POS_MOCAP; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +} + +/** + * @brief Pack a att_pos_mocap message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param x X position in meters (NED) + * @param y Y position in meters (NED) + * @param z Z position in meters (NED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,const float *q,float x,float y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#else + mavlink_att_pos_mocap_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATT_POS_MOCAP; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +} + +/** + * @brief Encode a att_pos_mocap struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param att_pos_mocap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_att_pos_mocap_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) +{ + return mavlink_msg_att_pos_mocap_pack(system_id, component_id, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); +} + +/** + * @brief Encode a att_pos_mocap struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param att_pos_mocap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) +{ + return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); +} + +/** + * @brief Send a att_pos_mocap message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param x X position in meters (NED) + * @param y Y position in meters (NED) + * @param z Z position in meters (NED) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + _mav_put_float_array(buf, 8, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +#else + mavlink_att_pos_mocap_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + _mav_put_float_array(buf, 8, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +#else + mavlink_att_pos_mocap_t *packet = (mavlink_att_pos_mocap_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATT_POS_MOCAP UNPACKING + + +/** + * @brief Get field time_usec from att_pos_mocap message + * + * @return Timestamp (micros since boot or Unix epoch) + */ +static inline uint64_t mavlink_msg_att_pos_mocap_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field q from att_pos_mocap message + * + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_att_pos_mocap_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 8); +} + +/** + * @brief Get field x from att_pos_mocap message + * + * @return X position in meters (NED) + */ +static inline float mavlink_msg_att_pos_mocap_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field y from att_pos_mocap message + * + * @return Y position in meters (NED) + */ +static inline float mavlink_msg_att_pos_mocap_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field z from att_pos_mocap message + * + * @return Z position in meters (NED) + */ +static inline float mavlink_msg_att_pos_mocap_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a att_pos_mocap message into a struct + * + * @param msg The message to decode + * @param att_pos_mocap C-struct to decode the message contents into + */ +static inline void mavlink_msg_att_pos_mocap_decode(const mavlink_message_t* msg, mavlink_att_pos_mocap_t* att_pos_mocap) +{ +#if MAVLINK_NEED_BYTE_SWAP + att_pos_mocap->time_usec = mavlink_msg_att_pos_mocap_get_time_usec(msg); + mavlink_msg_att_pos_mocap_get_q(msg, att_pos_mocap->q); + att_pos_mocap->x = mavlink_msg_att_pos_mocap_get_x(msg); + att_pos_mocap->y = mavlink_msg_att_pos_mocap_get_y(msg); + att_pos_mocap->z = mavlink_msg_att_pos_mocap_get_z(msg); +#else + memcpy(att_pos_mocap, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_attitude.h b/flightcode/mavlink/c_library/common/mavlink_msg_attitude.h new file mode 100644 index 0000000..ff2f104 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_attitude.h @@ -0,0 +1,353 @@ +// MESSAGE ATTITUDE PACKING + +#define MAVLINK_MSG_ID_ATTITUDE 30 + +typedef struct __mavlink_attitude_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float roll; ///< Roll angle (rad, -pi..+pi) + float pitch; ///< Pitch angle (rad, -pi..+pi) + float yaw; ///< Yaw angle (rad, -pi..+pi) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) +} mavlink_attitude_t; + +#define MAVLINK_MSG_ID_ATTITUDE_LEN 28 +#define MAVLINK_MSG_ID_30_LEN 28 + +#define MAVLINK_MSG_ID_ATTITUDE_CRC 39 +#define MAVLINK_MSG_ID_30_CRC 39 + + + +#define MAVLINK_MESSAGE_INFO_ATTITUDE { \ + "ATTITUDE", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, yaw) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, yawspeed) }, \ + } \ +} + + +/** + * @brief Pack a attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param roll Roll angle (rad, -pi..+pi) + * @param pitch Pitch angle (rad, -pi..+pi) + * @param yaw Yaw angle (rad, -pi..+pi) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +} + +/** + * @brief Pack a attitude message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param roll Roll angle (rad, -pi..+pi) + * @param pitch Pitch angle (rad, -pi..+pi) + * @param yaw Yaw angle (rad, -pi..+pi) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +} + +/** + * @brief Encode a attitude struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + +/** + * @brief Encode a attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + +/** + * @brief Send a attitude message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param roll Roll angle (rad, -pi..+pi) + * @param pitch Pitch angle (rad, -pi..+pi) + * @param yaw Yaw angle (rad, -pi..+pi) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#else + mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field roll from attitude message + * + * @return Roll angle (rad, -pi..+pi) + */ +static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field pitch from attitude message + * + * @return Pitch angle (rad, -pi..+pi) + */ +static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from attitude message + * + * @return Yaw angle (rad, -pi..+pi) + */ +static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field rollspeed from attitude message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field pitchspeed from attitude message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field yawspeed from attitude message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a attitude message into a struct + * + * @param msg The message to decode + * @param attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude->time_boot_ms = mavlink_msg_attitude_get_time_boot_ms(msg); + attitude->roll = mavlink_msg_attitude_get_roll(msg); + attitude->pitch = mavlink_msg_attitude_get_pitch(msg); + attitude->yaw = mavlink_msg_attitude_get_yaw(msg); + attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); + attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); + attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); +#else + memcpy(attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion.h b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion.h new file mode 100644 index 0000000..35170a7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion.h @@ -0,0 +1,377 @@ +// MESSAGE ATTITUDE_QUATERNION PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31 + +typedef struct __mavlink_attitude_quaternion_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float q1; ///< Quaternion component 1, w (1 in null-rotation) + float q2; ///< Quaternion component 2, x (0 in null-rotation) + float q3; ///< Quaternion component 3, y (0 in null-rotation) + float q4; ///< Quaternion component 4, z (0 in null-rotation) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) +} mavlink_attitude_quaternion_t; + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32 +#define MAVLINK_MSG_ID_31_LEN 32 + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246 +#define MAVLINK_MSG_ID_31_CRC 246 + + + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ + "ATTITUDE_QUATERNION", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ + { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ + { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ + { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \ + { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_quaternion_t, q4) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ + } \ +} + + +/** + * @brief Pack a attitude_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#else + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +} + +/** + * @brief Pack a attitude_quaternion message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#else + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +} + +/** + * @brief Encode a attitude_quaternion struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); +} + +/** + * @brief Encode a attitude_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); +} + +/** + * @brief Send a attitude_quaternion message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#else + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#else + mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_QUATERNION UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_quaternion message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field q1 from attitude_quaternion message + * + * @return Quaternion component 1, w (1 in null-rotation) + */ +static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field q2 from attitude_quaternion message + * + * @return Quaternion component 2, x (0 in null-rotation) + */ +static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field q3 from attitude_quaternion message + * + * @return Quaternion component 3, y (0 in null-rotation) + */ +static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field q4 from attitude_quaternion message + * + * @return Quaternion component 4, z (0 in null-rotation) + */ +static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rollspeed from attitude_quaternion message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from attitude_quaternion message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from attitude_quaternion message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a attitude_quaternion message into a struct + * + * @param msg The message to decode + * @param attitude_quaternion C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_t* attitude_quaternion) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg); + attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg); + attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg); + attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg); + attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg); + attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg); + attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg); + attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg); +#else + memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion_cov.h b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion_cov.h new file mode 100644 index 0000000..38b8f24 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_quaternion_cov.h @@ -0,0 +1,322 @@ +// MESSAGE ATTITUDE_QUATERNION_COV PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV 61 + +typedef struct __mavlink_attitude_quaternion_cov_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float q[4]; ///< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + float covariance[9]; ///< Attitude covariance +} mavlink_attitude_quaternion_cov_t; + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN 68 +#define MAVLINK_MSG_ID_61_LEN 68 + +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC 153 +#define MAVLINK_MSG_ID_61_CRC 153 + +#define MAVLINK_MSG_ATTITUDE_QUATERNION_COV_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ATTITUDE_QUATERNION_COV_FIELD_COVARIANCE_LEN 9 + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV { \ + "ATTITUDE_QUATERNION_COV", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_cov_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_attitude_quaternion_cov_t, q) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_cov_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_cov_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_cov_t, yawspeed) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 32, offsetof(mavlink_attitude_quaternion_cov_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a attitude_quaternion_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} + +/** + * @brief Pack a attitude_quaternion_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const float *q,float rollspeed,float pitchspeed,float yawspeed,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} + +/** + * @brief Encode a attitude_quaternion_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ + return mavlink_msg_attitude_quaternion_cov_pack(system_id, component_id, msg, attitude_quaternion_cov->time_boot_ms, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); +} + +/** + * @brief Encode a attitude_quaternion_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ + return mavlink_msg_attitude_quaternion_cov_pack_chan(system_id, component_id, chan, msg, attitude_quaternion_cov->time_boot_ms, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); +} + +/** + * @brief Send a attitude_quaternion_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param covariance Attitude covariance + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_quaternion_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_boot_ms = time_boot_ms; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_quaternion_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 32, covariance, 9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#else + mavlink_attitude_quaternion_cov_t *packet = (mavlink_attitude_quaternion_cov_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_QUATERNION_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_quaternion_cov message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_attitude_quaternion_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field q from attitude_quaternion_cov message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field rollspeed from attitude_quaternion_cov message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from attitude_quaternion_cov message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from attitude_quaternion_cov message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_quaternion_cov_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field covariance from attitude_quaternion_cov message + * + * @return Attitude covariance + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 9, 32); +} + +/** + * @brief Decode a attitude_quaternion_cov message into a struct + * + * @param msg The message to decode + * @param attitude_quaternion_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_quaternion_cov_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_quaternion_cov->time_boot_ms = mavlink_msg_attitude_quaternion_cov_get_time_boot_ms(msg); + mavlink_msg_attitude_quaternion_cov_get_q(msg, attitude_quaternion_cov->q); + attitude_quaternion_cov->rollspeed = mavlink_msg_attitude_quaternion_cov_get_rollspeed(msg); + attitude_quaternion_cov->pitchspeed = mavlink_msg_attitude_quaternion_cov_get_pitchspeed(msg); + attitude_quaternion_cov->yawspeed = mavlink_msg_attitude_quaternion_cov_get_yawspeed(msg); + mavlink_msg_attitude_quaternion_cov_get_covariance(msg, attitude_quaternion_cov->covariance); +#else + memcpy(attitude_quaternion_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_attitude_target.h b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_target.h new file mode 100644 index 0000000..7c4b1c7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_attitude_target.h @@ -0,0 +1,345 @@ +// MESSAGE ATTITUDE_TARGET PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET 83 + +typedef struct __mavlink_attitude_target_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float q[4]; ///< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float body_roll_rate; ///< Body roll rate in radians per second + float body_pitch_rate; ///< Body roll rate in radians per second + float body_yaw_rate; ///< Body roll rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + uint8_t type_mask; ///< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude +} mavlink_attitude_target_t; + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN 37 +#define MAVLINK_MSG_ID_83_LEN 37 + +#define MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC 22 +#define MAVLINK_MSG_ID_83_CRC 22 + +#define MAVLINK_MSG_ATTITUDE_TARGET_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET { \ + "ATTITUDE_TARGET", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_target_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_attitude_target_t, q) }, \ + { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_target_t, body_roll_rate) }, \ + { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_target_t, body_pitch_rate) }, \ + { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_target_t, body_yaw_rate) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_attitude_target_t, thrust) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_attitude_target_t, type_mask) }, \ + } \ +} + + +/** + * @brief Pack a attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Pack a attitude_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Encode a attitude_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_target_t* attitude_target) +{ + return mavlink_msg_attitude_target_pack(system_id, component_id, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); +} + +/** + * @brief Encode a attitude_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_target_t* attitude_target) +{ + return mavlink_msg_attitude_target_pack_chan(system_id, component_id, chan, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); +} + +/** + * @brief Send a attitude_target message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_attitude_target_t *packet = (mavlink_attitude_target_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->body_roll_rate = body_roll_rate; + packet->body_pitch_rate = body_pitch_rate; + packet->body_yaw_rate = body_yaw_rate; + packet->thrust = thrust; + packet->type_mask = type_mask; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_TARGET UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_target message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_attitude_target_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field type_mask from attitude_target message + * + * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + */ +static inline uint8_t mavlink_msg_attitude_target_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field q from attitude_target message + * + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_attitude_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field body_roll_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_roll_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field body_pitch_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field body_yaw_rate from attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_target_get_body_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field thrust from attitude_target message + * + * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +static inline float mavlink_msg_attitude_target_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a attitude_target message into a struct + * + * @param msg The message to decode + * @param attitude_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_target_decode(const mavlink_message_t* msg, mavlink_attitude_target_t* attitude_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_target->time_boot_ms = mavlink_msg_attitude_target_get_time_boot_ms(msg); + mavlink_msg_attitude_target_get_q(msg, attitude_target->q); + attitude_target->body_roll_rate = mavlink_msg_attitude_target_get_body_roll_rate(msg); + attitude_target->body_pitch_rate = mavlink_msg_attitude_target_get_body_pitch_rate(msg); + attitude_target->body_yaw_rate = mavlink_msg_attitude_target_get_body_yaw_rate(msg); + attitude_target->thrust = mavlink_msg_attitude_target_get_thrust(msg); + attitude_target->type_mask = mavlink_msg_attitude_target_get_type_mask(msg); +#else + memcpy(attitude_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_auth_key.h b/flightcode/mavlink/c_library/common/mavlink_msg_auth_key.h new file mode 100644 index 0000000..f31b6bb --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_auth_key.h @@ -0,0 +1,209 @@ +// MESSAGE AUTH_KEY PACKING + +#define MAVLINK_MSG_ID_AUTH_KEY 7 + +typedef struct __mavlink_auth_key_t +{ + char key[32]; ///< key +} mavlink_auth_key_t; + +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_ID_7_LEN 32 + +#define MAVLINK_MSG_ID_AUTH_KEY_CRC 119 +#define MAVLINK_MSG_ID_7_CRC 119 + +#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 + +#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ + "AUTH_KEY", \ + 1, \ + { { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ + } \ +} + + +/** + * @brief Pack a auth_key message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +} + +/** + * @brief Pack a auth_key message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +} + +/** + * @brief Encode a auth_key struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); +} + +/** + * @brief Encode a auth_key struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); +} + +/** + * @brief Send a auth_key message + * @param chan MAVLink channel to send the message + * + * @param key key + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; + + _mav_put_char_array(buf, 0, key, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_char_array(buf, 0, key, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#else + mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; + + mav_array_memcpy(packet->key, key, sizeof(char)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AUTH_KEY UNPACKING + + +/** + * @brief Get field key from auth_key message + * + * @return key + */ +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) +{ + return _MAV_RETURN_char_array(msg, key, 32, 0); +} + +/** + * @brief Decode a auth_key message into a struct + * + * @param msg The message to decode + * @param auth_key C-struct to decode the message contents into + */ +static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_auth_key_get_key(msg, auth_key->key); +#else + memcpy(auth_key, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_autopilot_version.h b/flightcode/mavlink/c_library/common/mavlink_msg_autopilot_version.h new file mode 100644 index 0000000..5012503 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_autopilot_version.h @@ -0,0 +1,443 @@ +// MESSAGE AUTOPILOT_VERSION PACKING + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION 148 + +typedef struct __mavlink_autopilot_version_t +{ + uint64_t capabilities; ///< bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + uint64_t uid; ///< UID if provided by hardware + uint32_t flight_sw_version; ///< Firmware version number + uint32_t middleware_sw_version; ///< Middleware version number + uint32_t os_sw_version; ///< Operating system version number + uint32_t board_version; ///< HW / board version (last 8 bytes should be silicon ID, if any) + uint16_t vendor_id; ///< ID of the board vendor + uint16_t product_id; ///< ID of the product + uint8_t flight_custom_version[8]; ///< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + uint8_t middleware_custom_version[8]; ///< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + uint8_t os_custom_version[8]; ///< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. +} mavlink_autopilot_version_t; + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 60 +#define MAVLINK_MSG_ID_148_LEN 60 + +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC 178 +#define MAVLINK_MSG_ID_148_CRC 178 + +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_FLIGHT_CUSTOM_VERSION_LEN 8 +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_MIDDLEWARE_CUSTOM_VERSION_LEN 8 +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_OS_CUSTOM_VERSION_LEN 8 + +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ + "AUTOPILOT_VERSION", \ + 11, \ + { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ + { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_autopilot_version_t, uid) }, \ + { "flight_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_autopilot_version_t, flight_sw_version) }, \ + { "middleware_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_autopilot_version_t, middleware_sw_version) }, \ + { "os_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_autopilot_version_t, os_sw_version) }, \ + { "board_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_autopilot_version_t, board_version) }, \ + { "vendor_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_autopilot_version_t, vendor_id) }, \ + { "product_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_autopilot_version_t, product_id) }, \ + { "flight_custom_version", NULL, MAVLINK_TYPE_UINT8_T, 8, 36, offsetof(mavlink_autopilot_version_t, flight_custom_version) }, \ + { "middleware_custom_version", NULL, MAVLINK_TYPE_UINT8_T, 8, 44, offsetof(mavlink_autopilot_version_t, middleware_custom_version) }, \ + { "os_custom_version", NULL, MAVLINK_TYPE_UINT8_T, 8, 52, offsetof(mavlink_autopilot_version_t, os_custom_version) }, \ + } \ +} + + +/** + * @brief Pack a autopilot_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param flight_sw_version Firmware version number + * @param middleware_sw_version Middleware version number + * @param os_sw_version Operating system version number + * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param vendor_id ID of the board vendor + * @param product_id ID of the product + * @param uid UID if provided by hardware + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint64_t(buf, 8, uid); + _mav_put_uint32_t(buf, 16, flight_sw_version); + _mav_put_uint32_t(buf, 20, middleware_sw_version); + _mav_put_uint32_t(buf, 24, os_sw_version); + _mav_put_uint32_t(buf, 28, board_version); + _mav_put_uint16_t(buf, 32, vendor_id); + _mav_put_uint16_t(buf, 34, product_id); + _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); + _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); + _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.uid = uid; + packet.flight_sw_version = flight_sw_version; + packet.middleware_sw_version = middleware_sw_version; + packet.os_sw_version = os_sw_version; + packet.board_version = board_version; + packet.vendor_id = vendor_id; + packet.product_id = product_id; + mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} + +/** + * @brief Pack a autopilot_version message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param flight_sw_version Firmware version number + * @param middleware_sw_version Middleware version number + * @param os_sw_version Operating system version number + * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param vendor_id ID of the board vendor + * @param product_id ID of the product + * @param uid UID if provided by hardware + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t capabilities,uint32_t flight_sw_version,uint32_t middleware_sw_version,uint32_t os_sw_version,uint32_t board_version,const uint8_t *flight_custom_version,const uint8_t *middleware_custom_version,const uint8_t *os_custom_version,uint16_t vendor_id,uint16_t product_id,uint64_t uid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint64_t(buf, 8, uid); + _mav_put_uint32_t(buf, 16, flight_sw_version); + _mav_put_uint32_t(buf, 20, middleware_sw_version); + _mav_put_uint32_t(buf, 24, os_sw_version); + _mav_put_uint32_t(buf, 28, board_version); + _mav_put_uint16_t(buf, 32, vendor_id); + _mav_put_uint16_t(buf, 34, product_id); + _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); + _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); + _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.uid = uid; + packet.flight_sw_version = flight_sw_version; + packet.middleware_sw_version = middleware_sw_version; + packet.os_sw_version = os_sw_version; + packet.board_version = board_version; + packet.vendor_id = vendor_id; + packet.product_id = product_id; + mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} + +/** + * @brief Encode a autopilot_version struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param autopilot_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) +{ + return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); +} + +/** + * @brief Encode a autopilot_version struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param autopilot_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) +{ + return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); +} + +/** + * @brief Send a autopilot_version message + * @param chan MAVLink channel to send the message + * + * @param capabilities bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + * @param flight_sw_version Firmware version number + * @param middleware_sw_version Middleware version number + * @param os_sw_version Operating system version number + * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param vendor_id ID of the board vendor + * @param product_id ID of the product + * @param uid UID if provided by hardware + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint64_t(buf, 8, uid); + _mav_put_uint32_t(buf, 16, flight_sw_version); + _mav_put_uint32_t(buf, 20, middleware_sw_version); + _mav_put_uint32_t(buf, 24, os_sw_version); + _mav_put_uint32_t(buf, 28, board_version); + _mav_put_uint16_t(buf, 32, vendor_id); + _mav_put_uint16_t(buf, 34, product_id); + _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); + _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); + _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.uid = uid; + packet.flight_sw_version = flight_sw_version; + packet.middleware_sw_version = middleware_sw_version; + packet.os_sw_version = os_sw_version; + packet.board_version = board_version; + packet.vendor_id = vendor_id; + packet.product_id = product_id; + mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint64_t(buf, 8, uid); + _mav_put_uint32_t(buf, 16, flight_sw_version); + _mav_put_uint32_t(buf, 20, middleware_sw_version); + _mav_put_uint32_t(buf, 24, os_sw_version); + _mav_put_uint32_t(buf, 28, board_version); + _mav_put_uint16_t(buf, 32, vendor_id); + _mav_put_uint16_t(buf, 34, product_id); + _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); + _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); + _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#else + mavlink_autopilot_version_t *packet = (mavlink_autopilot_version_t *)msgbuf; + packet->capabilities = capabilities; + packet->uid = uid; + packet->flight_sw_version = flight_sw_version; + packet->middleware_sw_version = middleware_sw_version; + packet->os_sw_version = os_sw_version; + packet->board_version = board_version; + packet->vendor_id = vendor_id; + packet->product_id = product_id; + mav_array_memcpy(packet->flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet->middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet->os_custom_version, os_custom_version, sizeof(uint8_t)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE AUTOPILOT_VERSION UNPACKING + + +/** + * @brief Get field capabilities from autopilot_version message + * + * @return bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) + */ +static inline uint64_t mavlink_msg_autopilot_version_get_capabilities(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field flight_sw_version from autopilot_version message + * + * @return Firmware version number + */ +static inline uint32_t mavlink_msg_autopilot_version_get_flight_sw_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field middleware_sw_version from autopilot_version message + * + * @return Middleware version number + */ +static inline uint32_t mavlink_msg_autopilot_version_get_middleware_sw_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field os_sw_version from autopilot_version message + * + * @return Operating system version number + */ +static inline uint32_t mavlink_msg_autopilot_version_get_os_sw_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 24); +} + +/** + * @brief Get field board_version from autopilot_version message + * + * @return HW / board version (last 8 bytes should be silicon ID, if any) + */ +static inline uint32_t mavlink_msg_autopilot_version_get_board_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + +/** + * @brief Get field flight_custom_version from autopilot_version message + * + * @return Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + */ +static inline uint16_t mavlink_msg_autopilot_version_get_flight_custom_version(const mavlink_message_t* msg, uint8_t *flight_custom_version) +{ + return _MAV_RETURN_uint8_t_array(msg, flight_custom_version, 8, 36); +} + +/** + * @brief Get field middleware_custom_version from autopilot_version message + * + * @return Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + */ +static inline uint16_t mavlink_msg_autopilot_version_get_middleware_custom_version(const mavlink_message_t* msg, uint8_t *middleware_custom_version) +{ + return _MAV_RETURN_uint8_t_array(msg, middleware_custom_version, 8, 44); +} + +/** + * @brief Get field os_custom_version from autopilot_version message + * + * @return Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + */ +static inline uint16_t mavlink_msg_autopilot_version_get_os_custom_version(const mavlink_message_t* msg, uint8_t *os_custom_version) +{ + return _MAV_RETURN_uint8_t_array(msg, os_custom_version, 8, 52); +} + +/** + * @brief Get field vendor_id from autopilot_version message + * + * @return ID of the board vendor + */ +static inline uint16_t mavlink_msg_autopilot_version_get_vendor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field product_id from autopilot_version message + * + * @return ID of the product + */ +static inline uint16_t mavlink_msg_autopilot_version_get_product_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field uid from autopilot_version message + * + * @return UID if provided by hardware + */ +static inline uint64_t mavlink_msg_autopilot_version_get_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 8); +} + +/** + * @brief Decode a autopilot_version message into a struct + * + * @param msg The message to decode + * @param autopilot_version C-struct to decode the message contents into + */ +static inline void mavlink_msg_autopilot_version_decode(const mavlink_message_t* msg, mavlink_autopilot_version_t* autopilot_version) +{ +#if MAVLINK_NEED_BYTE_SWAP + autopilot_version->capabilities = mavlink_msg_autopilot_version_get_capabilities(msg); + autopilot_version->uid = mavlink_msg_autopilot_version_get_uid(msg); + autopilot_version->flight_sw_version = mavlink_msg_autopilot_version_get_flight_sw_version(msg); + autopilot_version->middleware_sw_version = mavlink_msg_autopilot_version_get_middleware_sw_version(msg); + autopilot_version->os_sw_version = mavlink_msg_autopilot_version_get_os_sw_version(msg); + autopilot_version->board_version = mavlink_msg_autopilot_version_get_board_version(msg); + autopilot_version->vendor_id = mavlink_msg_autopilot_version_get_vendor_id(msg); + autopilot_version->product_id = mavlink_msg_autopilot_version_get_product_id(msg); + mavlink_msg_autopilot_version_get_flight_custom_version(msg, autopilot_version->flight_custom_version); + mavlink_msg_autopilot_version_get_middleware_custom_version(msg, autopilot_version->middleware_custom_version); + mavlink_msg_autopilot_version_get_os_custom_version(msg, autopilot_version->os_custom_version); +#else + memcpy(autopilot_version, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_battery_status.h b/flightcode/mavlink/c_library/common/mavlink_msg_battery_status.h new file mode 100644 index 0000000..12371e6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_battery_status.h @@ -0,0 +1,393 @@ +// MESSAGE BATTERY_STATUS PACKING + +#define MAVLINK_MSG_ID_BATTERY_STATUS 147 + +typedef struct __mavlink_battery_status_t +{ + int32_t current_consumed; ///< Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + int32_t energy_consumed; ///< Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + int16_t temperature; ///< Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + uint16_t voltages[10]; ///< Battery voltage of cells, in millivolts (1 = 1 millivolt) + int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + uint8_t id; ///< Battery ID + uint8_t battery_function; ///< Function of the battery + uint8_t type; ///< Type (chemistry) of the battery + int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery +} mavlink_battery_status_t; + +#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 36 +#define MAVLINK_MSG_ID_147_LEN 36 + +#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 154 +#define MAVLINK_MSG_ID_147_CRC 154 + +#define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN 10 + +#define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ + "BATTERY_STATUS", \ + 9, \ + { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ + { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_battery_status_t, temperature) }, \ + { "voltages", NULL, MAVLINK_TYPE_UINT16_T, 10, 10, offsetof(mavlink_battery_status_t, voltages) }, \ + { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_battery_status_t, current_battery) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ + { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + } \ +} + + +/** + * @brief Pack a battery_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#else + mavlink_battery_status_t packet; + packet.current_consumed = current_consumed; + packet.energy_consumed = energy_consumed; + packet.temperature = temperature; + packet.current_battery = current_battery; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.battery_remaining = battery_remaining; + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +} + +/** + * @brief Pack a battery_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#else + mavlink_battery_status_t packet; + packet.current_consumed = current_consumed; + packet.energy_consumed = energy_consumed; + packet.temperature = temperature; + packet.current_battery = current_battery; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.battery_remaining = battery_remaining; + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +} + +/** + * @brief Encode a battery_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); +} + +/** + * @brief Encode a battery_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); +} + +/** + * @brief Send a battery_status message + * @param chan MAVLink channel to send the message + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + * @param voltages Battery voltage of cells, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#else + mavlink_battery_status_t packet; + packet.current_consumed = current_consumed; + packet.energy_consumed = energy_consumed; + packet.temperature = temperature; + packet.current_battery = current_battery; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.battery_remaining = battery_remaining; + mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_uint16_t_array(buf, 10, voltages, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#else + mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; + packet->current_consumed = current_consumed; + packet->energy_consumed = energy_consumed; + packet->temperature = temperature; + packet->current_battery = current_battery; + packet->id = id; + packet->battery_function = battery_function; + packet->type = type; + packet->battery_remaining = battery_remaining; + mav_array_memcpy(packet->voltages, voltages, sizeof(uint16_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE BATTERY_STATUS UNPACKING + + +/** + * @brief Get field id from battery_status message + * + * @return Battery ID + */ +static inline uint8_t mavlink_msg_battery_status_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field battery_function from battery_status message + * + * @return Function of the battery + */ +static inline uint8_t mavlink_msg_battery_status_get_battery_function(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field type from battery_status message + * + * @return Type (chemistry) of the battery + */ +static inline uint8_t mavlink_msg_battery_status_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field temperature from battery_status message + * + * @return Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature. + */ +static inline int16_t mavlink_msg_battery_status_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field voltages from battery_status message + * + * @return Battery voltage of cells, in millivolts (1 = 1 millivolt) + */ +static inline uint16_t mavlink_msg_battery_status_get_voltages(const mavlink_message_t* msg, uint16_t *voltages) +{ + return _MAV_RETURN_uint16_t_array(msg, voltages, 10, 10); +} + +/** + * @brief Get field current_battery from battery_status message + * + * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + */ +static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 30); +} + +/** + * @brief Get field current_consumed from battery_status message + * + * @return Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + */ +static inline int32_t mavlink_msg_battery_status_get_current_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field energy_consumed from battery_status message + * + * @return Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + */ +static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field battery_remaining from battery_status message + * + * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery + */ +static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 35); +} + +/** + * @brief Decode a battery_status message into a struct + * + * @param msg The message to decode + * @param battery_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* msg, mavlink_battery_status_t* battery_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + battery_status->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg); + battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg); + battery_status->temperature = mavlink_msg_battery_status_get_temperature(msg); + mavlink_msg_battery_status_get_voltages(msg, battery_status->voltages); + battery_status->current_battery = mavlink_msg_battery_status_get_current_battery(msg); + battery_status->id = mavlink_msg_battery_status_get_id(msg); + battery_status->battery_function = mavlink_msg_battery_status_get_battery_function(msg); + battery_status->type = mavlink_msg_battery_status_get_type(msg); + battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); +#else + memcpy(battery_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_camera_trigger.h b/flightcode/mavlink/c_library/common/mavlink_msg_camera_trigger.h new file mode 100644 index 0000000..e060e27 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_camera_trigger.h @@ -0,0 +1,233 @@ +// MESSAGE CAMERA_TRIGGER PACKING + +#define MAVLINK_MSG_ID_CAMERA_TRIGGER 112 + +typedef struct __mavlink_camera_trigger_t +{ + uint64_t time_usec; ///< Timestamp for the image frame in microseconds + uint32_t seq; ///< Image frame sequence +} mavlink_camera_trigger_t; + +#define MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN 12 +#define MAVLINK_MSG_ID_112_LEN 12 + +#define MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC 174 +#define MAVLINK_MSG_ID_112_CRC 174 + + + +#define MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER { \ + "CAMERA_TRIGGER", \ + 2, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_trigger_t, time_usec) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_trigger_t, seq) }, \ + } \ +} + + +/** + * @brief Pack a camera_trigger message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp for the image frame in microseconds + * @param seq Image frame sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_trigger_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#else + mavlink_camera_trigger_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRIGGER; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +} + +/** + * @brief Pack a camera_trigger message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp for the image frame in microseconds + * @param seq Image frame sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_trigger_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#else + mavlink_camera_trigger_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRIGGER; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +} + +/** + * @brief Encode a camera_trigger struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_trigger C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_trigger_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_trigger_t* camera_trigger) +{ + return mavlink_msg_camera_trigger_pack(system_id, component_id, msg, camera_trigger->time_usec, camera_trigger->seq); +} + +/** + * @brief Encode a camera_trigger struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_trigger C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_trigger_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_trigger_t* camera_trigger) +{ + return mavlink_msg_camera_trigger_pack_chan(system_id, component_id, chan, msg, camera_trigger->time_usec, camera_trigger->seq); +} + +/** + * @brief Send a camera_trigger message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp for the image frame in microseconds + * @param seq Image frame sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_trigger_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +#else + mavlink_camera_trigger_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_trigger_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +#else + mavlink_camera_trigger_t *packet = (mavlink_camera_trigger_t *)msgbuf; + packet->time_usec = time_usec; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRIGGER, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_TRIGGER UNPACKING + + +/** + * @brief Get field time_usec from camera_trigger message + * + * @return Timestamp for the image frame in microseconds + */ +static inline uint64_t mavlink_msg_camera_trigger_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field seq from camera_trigger message + * + * @return Image frame sequence + */ +static inline uint32_t mavlink_msg_camera_trigger_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Decode a camera_trigger message into a struct + * + * @param msg The message to decode + * @param camera_trigger C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_trigger_decode(const mavlink_message_t* msg, mavlink_camera_trigger_t* camera_trigger) +{ +#if MAVLINK_NEED_BYTE_SWAP + camera_trigger->time_usec = mavlink_msg_camera_trigger_get_time_usec(msg); + camera_trigger->seq = mavlink_msg_camera_trigger_get_seq(msg); +#else + memcpy(camera_trigger, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control.h b/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control.h new file mode 100644 index 0000000..3f0987f --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control.h @@ -0,0 +1,273 @@ +// MESSAGE CHANGE_OPERATOR_CONTROL PACKING + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 + +typedef struct __mavlink_change_operator_control_t +{ + uint8_t target_system; ///< System the GCS requests control for + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" +} mavlink_change_operator_control_t; + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_ID_5_LEN 28 + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC 217 +#define MAVLINK_MSG_ID_5_CRC 217 + +#define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 + +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \ + "CHANGE_OPERATOR_CONTROL", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \ + { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \ + { "version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \ + { "passkey", NULL, MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \ + } \ +} + + +/** + * @brief Pack a change_operator_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a change_operator_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a change_operator_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + +/** + * @brief Encode a change_operator_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + +/** + * @brief Send a change_operator_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#else + mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf; + packet->target_system = target_system; + packet->control_request = control_request; + packet->version = version; + mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING + + +/** + * @brief Get field target_system from change_operator_control message + * + * @return System the GCS requests control for + */ +static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field control_request from change_operator_control message + * + * @return 0: request control of this MAV, 1: Release control of this MAV + */ +static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field version from change_operator_control message + * + * @return 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + */ +static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field passkey from change_operator_control message + * + * @return Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + */ +static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey) +{ + return _MAV_RETURN_char_array(msg, passkey, 25, 3); +} + +/** + * @brief Decode a change_operator_control message into a struct + * + * @param msg The message to decode + * @param change_operator_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg); + change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg); + change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); + mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); +#else + memcpy(change_operator_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control_ack.h b/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control_ack.h new file mode 100644 index 0000000..768e7ed --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_change_operator_control_ack.h @@ -0,0 +1,257 @@ +// MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 + +typedef struct __mavlink_change_operator_control_ack_t +{ + uint8_t gcs_system_id; ///< ID of the GCS this message + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control +} mavlink_change_operator_control_ack_t; + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_ID_6_LEN 3 + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC 104 +#define MAVLINK_MSG_ID_6_CRC 104 + + + +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK { \ + "CHANGE_OPERATOR_CONTROL_ACK", \ + 3, \ + { { "gcs_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \ + { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \ + { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \ + } \ +} + + +/** + * @brief Pack a change_operator_control_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +} + +/** + * @brief Pack a change_operator_control_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t gcs_system_id,uint8_t control_request,uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +} + +/** + * @brief Encode a change_operator_control_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + +/** + * @brief Encode a change_operator_control_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + +/** + * @brief Send a change_operator_control_ack message + * @param chan MAVLink channel to send the message + * + * @param gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#else + mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf; + packet->gcs_system_id = gcs_system_id; + packet->control_request = control_request; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING + + +/** + * @brief Get field gcs_system_id from change_operator_control_ack message + * + * @return ID of the GCS this message + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field control_request from change_operator_control_ack message + * + * @return 0: request control of this MAV, 1: Release control of this MAV + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field ack from change_operator_control_ack message + * + * @return 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a change_operator_control_ack message into a struct + * + * @param msg The message to decode + * @param change_operator_control_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg); + change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); + change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); +#else + memcpy(change_operator_control_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_command_ack.h b/flightcode/mavlink/c_library/common/mavlink_msg_command_ack.h new file mode 100644 index 0000000..d3d1630 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_command_ack.h @@ -0,0 +1,233 @@ +// MESSAGE COMMAND_ACK PACKING + +#define MAVLINK_MSG_ID_COMMAND_ACK 77 + +typedef struct __mavlink_command_ack_t +{ + uint16_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t result; ///< See MAV_RESULT enum +} mavlink_command_ack_t; + +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3 +#define MAVLINK_MSG_ID_77_LEN 3 + +#define MAVLINK_MSG_ID_COMMAND_ACK_CRC 143 +#define MAVLINK_MSG_ID_77_CRC 143 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ + "COMMAND_ACK", \ + 2, \ + { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ + { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ + } \ +} + + +/** + * @brief Pack a command_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param command Command ID, as defined by MAV_CMD enum. + * @param result See MAV_RESULT enum + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +} + +/** + * @brief Pack a command_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command Command ID, as defined by MAV_CMD enum. + * @param result See MAV_RESULT enum + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t command,uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +} + +/** + * @brief Encode a command_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); +} + +/** + * @brief Encode a command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); +} + +/** + * @brief Send a command_ack message + * @param chan MAVLink channel to send the message + * + * @param command Command ID, as defined by MAV_CMD enum. + * @param result See MAV_RESULT enum + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#else + mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; + packet->command = command; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMMAND_ACK UNPACKING + + +/** + * @brief Get field command from command_ack message + * + * @return Command ID, as defined by MAV_CMD enum. + */ +static inline uint16_t mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field result from command_ack message + * + * @return See MAV_RESULT enum + */ +static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a command_ack message into a struct + * + * @param msg The message to decode + * @param command_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + command_ack->command = mavlink_msg_command_ack_get_command(msg); + command_ack->result = mavlink_msg_command_ack_get_result(msg); +#else + memcpy(command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_command_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_command_int.h new file mode 100644 index 0000000..4713d01 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_command_int.h @@ -0,0 +1,497 @@ +// MESSAGE COMMAND_INT PACKING + +#define MAVLINK_MSG_ID_COMMAND_INT 75 + +typedef struct __mavlink_command_int_t +{ + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum + int32_t x; ///< PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + int32_t y; ///< PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + float z; ///< PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + uint16_t command; ///< The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp +} mavlink_command_int_t; + +#define MAVLINK_MSG_ID_COMMAND_INT_LEN 35 +#define MAVLINK_MSG_ID_75_LEN 35 + +#define MAVLINK_MSG_ID_COMMAND_INT_CRC 158 +#define MAVLINK_MSG_ID_75_CRC 158 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_INT { \ + "COMMAND_INT", \ + 13, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_int_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_int_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_int_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_int_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_command_int_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_command_int_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_int_t, z) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_int_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_int_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_int_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_command_int_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_command_int_t, autocontinue) }, \ + } \ +} + + +/** + * @brief Pack a command_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} + +/** + * @brief Pack a command_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} + +/** + * @brief Encode a command_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param command_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_int_t* command_int) +{ + return mavlink_msg_command_int_pack(system_id, component_id, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); +} + +/** + * @brief Encode a command_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_int_t* command_int) +{ + return mavlink_msg_command_int_pack_chan(system_id, component_id, chan, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); +} + +/** + * @brief Send a command_int message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_command_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMMAND_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#else + mavlink_command_int_t *packet = (mavlink_command_int_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)packet, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_INT, (const char *)packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMMAND_INT UNPACKING + + +/** + * @brief Get field target_system from command_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_command_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from command_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_command_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field frame from command_int message + * + * @return The coordinate system of the COMMAND. see MAV_FRAME in mavlink_types.h + */ +static inline uint8_t mavlink_msg_command_int_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field command from command_int message + * + * @return The scheduled action for the mission item. see MAV_CMD in common.xml MAVLink specs + */ +static inline uint16_t mavlink_msg_command_int_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field current from command_int message + * + * @return false:0, true:1 + */ +static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field autocontinue from command_int message + * + * @return autocontinue to next wp + */ +static inline uint8_t mavlink_msg_command_int_get_autocontinue(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field param1 from command_int message + * + * @return PARAM1, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from command_int message + * + * @return PARAM2, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from command_int message + * + * @return PARAM3, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from command_int message + * + * @return PARAM4, see MAV_CMD enum + */ +static inline float mavlink_msg_command_int_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field x from command_int message + * + * @return PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_command_int_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field y from command_int message + * + * @return PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_command_int_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field z from command_int message + * + * @return PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +static inline float mavlink_msg_command_int_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a command_int message into a struct + * + * @param msg The message to decode + * @param command_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_command_int_decode(const mavlink_message_t* msg, mavlink_command_int_t* command_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + command_int->param1 = mavlink_msg_command_int_get_param1(msg); + command_int->param2 = mavlink_msg_command_int_get_param2(msg); + command_int->param3 = mavlink_msg_command_int_get_param3(msg); + command_int->param4 = mavlink_msg_command_int_get_param4(msg); + command_int->x = mavlink_msg_command_int_get_x(msg); + command_int->y = mavlink_msg_command_int_get_y(msg); + command_int->z = mavlink_msg_command_int_get_z(msg); + command_int->command = mavlink_msg_command_int_get_command(msg); + command_int->target_system = mavlink_msg_command_int_get_target_system(msg); + command_int->target_component = mavlink_msg_command_int_get_target_component(msg); + command_int->frame = mavlink_msg_command_int_get_frame(msg); + command_int->current = mavlink_msg_command_int_get_current(msg); + command_int->autocontinue = mavlink_msg_command_int_get_autocontinue(msg); +#else + memcpy(command_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_command_long.h b/flightcode/mavlink/c_library/common/mavlink_msg_command_long.h new file mode 100644 index 0000000..161896b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_command_long.h @@ -0,0 +1,449 @@ +// MESSAGE COMMAND_LONG PACKING + +#define MAVLINK_MSG_ID_COMMAND_LONG 76 + +typedef struct __mavlink_command_long_t +{ + float param1; ///< Parameter 1, as defined by MAV_CMD enum. + float param2; ///< Parameter 2, as defined by MAV_CMD enum. + float param3; ///< Parameter 3, as defined by MAV_CMD enum. + float param4; ///< Parameter 4, as defined by MAV_CMD enum. + float param5; ///< Parameter 5, as defined by MAV_CMD enum. + float param6; ///< Parameter 6, as defined by MAV_CMD enum. + float param7; ///< Parameter 7, as defined by MAV_CMD enum. + uint16_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t target_system; ///< System which should execute the command + uint8_t target_component; ///< Component which should execute the command, 0 for all components + uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +} mavlink_command_long_t; + +#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 33 +#define MAVLINK_MSG_ID_76_LEN 33 + +#define MAVLINK_MSG_ID_COMMAND_LONG_CRC 152 +#define MAVLINK_MSG_ID_76_CRC 152 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_LONG { \ + "COMMAND_LONG", \ + 11, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_long_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_long_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_long_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_long_t, param4) }, \ + { "param5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_long_t, param5) }, \ + { "param6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_command_long_t, param6) }, \ + { "param7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_long_t, param7) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_long_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, target_component) }, \ + { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_long_t, confirmation) }, \ + } \ +} + + +/** + * @brief Pack a command_long message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID, as defined by MAV_CMD enum. + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1, as defined by MAV_CMD enum. + * @param param2 Parameter 2, as defined by MAV_CMD enum. + * @param param3 Parameter 3, as defined by MAV_CMD enum. + * @param param4 Parameter 4, as defined by MAV_CMD enum. + * @param param5 Parameter 5, as defined by MAV_CMD enum. + * @param param6 Parameter 6, as defined by MAV_CMD enum. + * @param param7 Parameter 7, as defined by MAV_CMD enum. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.confirmation = confirmation; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +} + +/** + * @brief Pack a command_long message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID, as defined by MAV_CMD enum. + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1, as defined by MAV_CMD enum. + * @param param2 Parameter 2, as defined by MAV_CMD enum. + * @param param3 Parameter 3, as defined by MAV_CMD enum. + * @param param4 Parameter 4, as defined by MAV_CMD enum. + * @param param5 Parameter 5, as defined by MAV_CMD enum. + * @param param6 Parameter 6, as defined by MAV_CMD enum. + * @param param7 Parameter 7, as defined by MAV_CMD enum. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t command,uint8_t confirmation,float param1,float param2,float param3,float param4,float param5,float param6,float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.confirmation = confirmation; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +} + +/** + * @brief Encode a command_long struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack(system_id, component_id, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + +/** + * @brief Encode a command_long struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + +/** + * @brief Send a command_long message + * @param chan MAVLink channel to send the message + * + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID, as defined by MAV_CMD enum. + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1, as defined by MAV_CMD enum. + * @param param2 Parameter 2, as defined by MAV_CMD enum. + * @param param3 Parameter 3, as defined by MAV_CMD enum. + * @param param4 Parameter 4, as defined by MAV_CMD enum. + * @param param5 Parameter 5, as defined by MAV_CMD enum. + * @param param6 Parameter 6, as defined by MAV_CMD enum. + * @param param7 Parameter 7, as defined by MAV_CMD enum. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.confirmation = confirmation; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#else + mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->param5 = param5; + packet->param6 = param6; + packet->param7 = param7; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->confirmation = confirmation; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMMAND_LONG UNPACKING + + +/** + * @brief Get field target_system from command_long message + * + * @return System which should execute the command + */ +static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from command_long message + * + * @return Component which should execute the command, 0 for all components + */ +static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field command from command_long message + * + * @return Command ID, as defined by MAV_CMD enum. + */ +static inline uint16_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field confirmation from command_long message + * + * @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + */ +static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field param1 from command_long message + * + * @return Parameter 1, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from command_long message + * + * @return Parameter 2, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from command_long message + * + * @return Parameter 3, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from command_long message + * + * @return Parameter 4, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field param5 from command_long message + * + * @return Parameter 5, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field param6 from command_long message + * + * @return Parameter 6, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field param7 from command_long message + * + * @return Parameter 7, as defined by MAV_CMD enum. + */ +static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a command_long message into a struct + * + * @param msg The message to decode + * @param command_long C-struct to decode the message contents into + */ +static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, mavlink_command_long_t* command_long) +{ +#if MAVLINK_NEED_BYTE_SWAP + command_long->param1 = mavlink_msg_command_long_get_param1(msg); + command_long->param2 = mavlink_msg_command_long_get_param2(msg); + command_long->param3 = mavlink_msg_command_long_get_param3(msg); + command_long->param4 = mavlink_msg_command_long_get_param4(msg); + command_long->param5 = mavlink_msg_command_long_get_param5(msg); + command_long->param6 = mavlink_msg_command_long_get_param6(msg); + command_long->param7 = mavlink_msg_command_long_get_param7(msg); + command_long->command = mavlink_msg_command_long_get_command(msg); + command_long->target_system = mavlink_msg_command_long_get_target_system(msg); + command_long->target_component = mavlink_msg_command_long_get_target_component(msg); + command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg); +#else + memcpy(command_long, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_data_stream.h b/flightcode/mavlink/c_library/common/mavlink_msg_data_stream.h new file mode 100644 index 0000000..640ffeb --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_data_stream.h @@ -0,0 +1,257 @@ +// MESSAGE DATA_STREAM PACKING + +#define MAVLINK_MSG_ID_DATA_STREAM 67 + +typedef struct __mavlink_data_stream_t +{ + uint16_t message_rate; ///< The requested interval between two messages of this type + uint8_t stream_id; ///< The ID of the requested data stream + uint8_t on_off; ///< 1 stream is enabled, 0 stream is stopped. +} mavlink_data_stream_t; + +#define MAVLINK_MSG_ID_DATA_STREAM_LEN 4 +#define MAVLINK_MSG_ID_67_LEN 4 + +#define MAVLINK_MSG_ID_DATA_STREAM_CRC 21 +#define MAVLINK_MSG_ID_67_CRC 21 + + + +#define MAVLINK_MESSAGE_INFO_DATA_STREAM { \ + "DATA_STREAM", \ + 3, \ + { { "message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_data_stream_t, message_rate) }, \ + { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_data_stream_t, stream_id) }, \ + { "on_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_data_stream_t, on_off) }, \ + } \ +} + + +/** + * @brief Pack a data_stream message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param stream_id The ID of the requested data stream + * @param message_rate The requested interval between two messages of this type + * @param on_off 1 stream is enabled, 0 stream is stopped. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +} + +/** + * @brief Pack a data_stream message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id The ID of the requested data stream + * @param message_rate The requested interval between two messages of this type + * @param on_off 1 stream is enabled, 0 stream is stopped. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint16_t message_rate,uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +} + +/** + * @brief Encode a data_stream struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack(system_id, component_id, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + +/** + * @brief Encode a data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + +/** + * @brief Send a data_stream message + * @param chan MAVLink channel to send the message + * + * @param stream_id The ID of the requested data stream + * @param message_rate The requested interval between two messages of this type + * @param on_off 1 stream is enabled, 0 stream is stopped. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#else + mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf; + packet->message_rate = message_rate; + packet->stream_id = stream_id; + packet->on_off = on_off; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA_STREAM UNPACKING + + +/** + * @brief Get field stream_id from data_stream message + * + * @return The ID of the requested data stream + */ +static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field message_rate from data_stream message + * + * @return The requested interval between two messages of this type + */ +static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field on_off from data_stream message + * + * @return 1 stream is enabled, 0 stream is stopped. + */ +static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Decode a data_stream message into a struct + * + * @param msg The message to decode + * @param data_stream C-struct to decode the message contents into + */ +static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* msg, mavlink_data_stream_t* data_stream) +{ +#if MAVLINK_NEED_BYTE_SWAP + data_stream->message_rate = mavlink_msg_data_stream_get_message_rate(msg); + data_stream->stream_id = mavlink_msg_data_stream_get_stream_id(msg); + data_stream->on_off = mavlink_msg_data_stream_get_on_off(msg); +#else + memcpy(data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_data_transmission_handshake.h b/flightcode/mavlink/c_library/common/mavlink_msg_data_transmission_handshake.h new file mode 100644 index 0000000..d84a737 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_data_transmission_handshake.h @@ -0,0 +1,353 @@ +// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING + +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130 + +typedef struct __mavlink_data_transmission_handshake_t +{ + uint32_t size; ///< total data size in bytes (set on ACK only) + uint16_t width; ///< Width of a matrix or image + uint16_t height; ///< Height of a matrix or image + uint16_t packets; ///< number of packets beeing sent (set on ACK only) + uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + uint8_t jpg_quality; ///< JPEG quality out of [1,100] +} mavlink_data_transmission_handshake_t; + +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 13 +#define MAVLINK_MSG_ID_130_LEN 13 + +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 29 +#define MAVLINK_MSG_ID_130_CRC 29 + + + +#define MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE { \ + "DATA_TRANSMISSION_HANDSHAKE", \ + 7, \ + { { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \ + { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, width) }, \ + { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, height) }, \ + { "packets", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, packets) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, type) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, payload) }, \ + { "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \ + } \ +} + + +/** + * @brief Pack a data_transmission_handshake message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + * @param size total data size in bytes (set on ACK only) + * @param width Width of a matrix or image + * @param height Height of a matrix or image + * @param packets number of packets beeing sent (set on ACK only) + * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + * @param jpg_quality JPEG quality out of [1,100] + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.width = width; + packet.height = height; + packet.packets = packets; + packet.type = type; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +} + +/** + * @brief Pack a data_transmission_handshake message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + * @param size total data size in bytes (set on ACK only) + * @param width Width of a matrix or image + * @param height Height of a matrix or image + * @param packets number of packets beeing sent (set on ACK only) + * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + * @param jpg_quality JPEG quality out of [1,100] + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint16_t packets,uint8_t payload,uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.width = width; + packet.height = height; + packet.packets = packets; + packet.type = type; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +} + +/** + * @brief Encode a data_transmission_handshake struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + +/** + * @brief Encode a data_transmission_handshake struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + +/** + * @brief Send a data_transmission_handshake message + * @param chan MAVLink channel to send the message + * + * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + * @param size total data size in bytes (set on ACK only) + * @param width Width of a matrix or image + * @param height Height of a matrix or image + * @param packets number of packets beeing sent (set on ACK only) + * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + * @param jpg_quality JPEG quality out of [1,100] + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.width = width; + packet.height = height; + packet.packets = packets; + packet.type = type; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#else + mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf; + packet->size = size; + packet->width = width; + packet->height = height; + packet->packets = packets; + packet->type = type; + packet->payload = payload; + packet->jpg_quality = jpg_quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING + + +/** + * @brief Get field type from data_transmission_handshake message + * + * @return type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + */ +static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field size from data_transmission_handshake message + * + * @return total data size in bytes (set on ACK only) + */ +static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field width from data_transmission_handshake message + * + * @return Width of a matrix or image + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_get_width(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field height from data_transmission_handshake message + * + * @return Height of a matrix or image + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field packets from data_transmission_handshake message + * + * @return number of packets beeing sent (set on ACK only) + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field payload from data_transmission_handshake message + * + * @return payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + */ +static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field jpg_quality from data_transmission_handshake message + * + * @return JPEG quality out of [1,100] + */ +static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Decode a data_transmission_handshake message into a struct + * + * @param msg The message to decode + * @param data_transmission_handshake C-struct to decode the message contents into + */ +static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ +#if MAVLINK_NEED_BYTE_SWAP + data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); + data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg); + data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg); + data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); + data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); + data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); + data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); +#else + memcpy(data_transmission_handshake, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_debug.h b/flightcode/mavlink/c_library/common/mavlink_msg_debug.h new file mode 100644 index 0000000..2102af8 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_debug.h @@ -0,0 +1,257 @@ +// MESSAGE DEBUG PACKING + +#define MAVLINK_MSG_ID_DEBUG 254 + +typedef struct __mavlink_debug_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable +} mavlink_debug_t; + +#define MAVLINK_MSG_ID_DEBUG_LEN 9 +#define MAVLINK_MSG_ID_254_LEN 9 + +#define MAVLINK_MSG_ID_DEBUG_CRC 46 +#define MAVLINK_MSG_ID_254_CRC 46 + + + +#define MAVLINK_MESSAGE_INFO_DEBUG { \ + "DEBUG", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \ + { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \ + { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \ + } \ +} + + +/** + * @brief Pack a debug message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param ind index of debug variable + * @param value DEBUG value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +} + +/** + * @brief Pack a debug message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param ind index of debug variable + * @param value DEBUG value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t ind,float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +} + +/** + * @brief Encode a debug struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value); +} + +/** + * @brief Encode a debug struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); +} + +/** + * @brief Send a debug message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param ind index of debug variable + * @param value DEBUG value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#else + mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + packet->ind = ind; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DEBUG UNPACKING + + +/** + * @brief Get field time_boot_ms from debug message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field ind from debug message + * + * @return index of debug variable + */ +static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field value from debug message + * + * @return DEBUG value + */ +static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Decode a debug message into a struct + * + * @param msg The message to decode + * @param debug C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) +{ +#if MAVLINK_NEED_BYTE_SWAP + debug->time_boot_ms = mavlink_msg_debug_get_time_boot_ms(msg); + debug->value = mavlink_msg_debug_get_value(msg); + debug->ind = mavlink_msg_debug_get_ind(msg); +#else + memcpy(debug, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_debug_vect.h b/flightcode/mavlink/c_library/common/mavlink_msg_debug_vect.h new file mode 100644 index 0000000..67c339e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_debug_vect.h @@ -0,0 +1,297 @@ +// MESSAGE DEBUG_VECT PACKING + +#define MAVLINK_MSG_ID_DEBUG_VECT 250 + +typedef struct __mavlink_debug_vect_t +{ + uint64_t time_usec; ///< Timestamp + float x; ///< x + float y; ///< y + float z; ///< z + char name[10]; ///< Name +} mavlink_debug_vect_t; + +#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_ID_250_LEN 30 + +#define MAVLINK_MSG_ID_DEBUG_VECT_CRC 49 +#define MAVLINK_MSG_ID_250_CRC 49 + +#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_DEBUG_VECT { \ + "DEBUG_VECT", \ + 5, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_vect_t, time_usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_debug_vect_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_debug_vect_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_debug_vect_t, z) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 20, offsetof(mavlink_debug_vect_t, name) }, \ + } \ +} + + +/** + * @brief Pack a debug_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param name Name + * @param time_usec Timestamp + * @param x x + * @param y y + * @param z z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +} + +/** + * @brief Pack a debug_vect message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param name Name + * @param time_usec Timestamp + * @param x x + * @param y y + * @param z z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *name,uint64_t time_usec,float x,float y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +} + +/** + * @brief Encode a debug_vect struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + +/** + * @brief Encode a debug_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + +/** + * @brief Send a debug_vect message + * @param chan MAVLink channel to send the message + * + * @param name Name + * @param time_usec Timestamp + * @param x x + * @param y y + * @param z z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#else + mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DEBUG_VECT UNPACKING + + +/** + * @brief Get field name from debug_vect message + * + * @return Name + */ +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 20); +} + +/** + * @brief Get field time_usec from debug_vect message + * + * @return Timestamp + */ +static inline uint64_t mavlink_msg_debug_vect_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field x from debug_vect message + * + * @return x + */ +static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from debug_vect message + * + * @return y + */ +static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from debug_vect message + * + * @return z + */ +static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a debug_vect message into a struct + * + * @param msg The message to decode + * @param debug_vect C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) +{ +#if MAVLINK_NEED_BYTE_SWAP + debug_vect->time_usec = mavlink_msg_debug_vect_get_time_usec(msg); + debug_vect->x = mavlink_msg_debug_vect_get_x(msg); + debug_vect->y = mavlink_msg_debug_vect_get_y(msg); + debug_vect->z = mavlink_msg_debug_vect_get_z(msg); + mavlink_msg_debug_vect_get_name(msg, debug_vect->name); +#else + memcpy(debug_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_distance_sensor.h b/flightcode/mavlink/c_library/common/mavlink_msg_distance_sensor.h new file mode 100644 index 0000000..a0e1a7c --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_distance_sensor.h @@ -0,0 +1,377 @@ +// MESSAGE DISTANCE_SENSOR PACKING + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 + +typedef struct __mavlink_distance_sensor_t +{ + uint32_t time_boot_ms; ///< Time since system boot + uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters + uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters + uint16_t current_distance; ///< Current distance reading + uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum. + uint8_t id; ///< Onboard ID of the sensor + uint8_t orientation; ///< Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings +} mavlink_distance_sensor_t; + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 14 + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 +#define MAVLINK_MSG_ID_132_CRC 85 + + + +#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ + "DISTANCE_SENSOR", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ + { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ + { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ + { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a distance_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a distance_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a distance_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); +} + +/** + * @brief Encode a distance_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); +} + +/** + * @brief Send a distance_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Time since system boot + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->current_distance = current_distance; + packet->type = type; + packet->id = id; + packet->orientation = orientation; + packet->covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DISTANCE_SENSOR UNPACKING + + +/** + * @brief Get field time_boot_ms from distance_sensor message + * + * @return Time since system boot + */ +static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field min_distance from distance_sensor message + * + * @return Minimum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field max_distance from distance_sensor message + * + * @return Maximum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field current_distance from distance_sensor message + * + * @return Current distance reading + */ +static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field type from distance_sensor message + * + * @return Type from MAV_DISTANCE_SENSOR enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field id from distance_sensor message + * + * @return Onboard ID of the sensor + */ +static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field orientation from distance_sensor message + * + * @return Direction the sensor faces from MAV_SENSOR_ORIENTATION enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field covariance from distance_sensor message + * + * @return Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Decode a distance_sensor message into a struct + * + * @param msg The message to decode + * @param distance_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg); + distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg); + distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg); + distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg); + distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg); + distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); + distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); + distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); +#else + memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_encapsulated_data.h b/flightcode/mavlink/c_library/common/mavlink_msg_encapsulated_data.h new file mode 100644 index 0000000..dacd7c9 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_encapsulated_data.h @@ -0,0 +1,225 @@ +// MESSAGE ENCAPSULATED_DATA PACKING + +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131 + +typedef struct __mavlink_encapsulated_data_t +{ + uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) + uint8_t data[253]; ///< image data bytes +} mavlink_encapsulated_data_t; + +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 +#define MAVLINK_MSG_ID_131_LEN 255 + +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223 +#define MAVLINK_MSG_ID_131_CRC 223 + +#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 + +#define MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA { \ + "ENCAPSULATED_DATA", \ + 2, \ + { { "seqnr", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_encapsulated_data_t, seqnr) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 253, 2, offsetof(mavlink_encapsulated_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a encapsulated_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param seqnr sequence number (starting with 0 on every transmission) + * @param data image data bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +} + +/** + * @brief Pack a encapsulated_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param seqnr sequence number (starting with 0 on every transmission) + * @param data image data bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t seqnr,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +} + +/** + * @brief Encode a encapsulated_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + +/** + * @brief Encode a encapsulated_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + +/** + * @brief Send a encapsulated_data message + * @param chan MAVLink channel to send the message + * + * @param seqnr sequence number (starting with 0 on every transmission) + * @param data image data bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#else + mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; + packet->seqnr = seqnr; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ENCAPSULATED_DATA UNPACKING + + +/** + * @brief Get field seqnr from encapsulated_data message + * + * @return sequence number (starting with 0 on every transmission) + */ +static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field data from encapsulated_data message + * + * @return image data bytes + */ +static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 253, 2); +} + +/** + * @brief Decode a encapsulated_data message into a struct + * + * @param msg The message to decode + * @param encapsulated_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); + mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); +#else + memcpy(encapsulated_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_file_transfer_protocol.h b/flightcode/mavlink/c_library/common/mavlink_msg_file_transfer_protocol.h new file mode 100644 index 0000000..f77cd94 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_file_transfer_protocol.h @@ -0,0 +1,273 @@ +// MESSAGE FILE_TRANSFER_PROTOCOL PACKING + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL 110 + +typedef struct __mavlink_file_transfer_protocol_t +{ + uint8_t target_network; ///< Network ID (0 for broadcast) + uint8_t target_system; ///< System ID (0 for broadcast) + uint8_t target_component; ///< Component ID (0 for broadcast) + uint8_t payload[251]; ///< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. +} mavlink_file_transfer_protocol_t; + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN 254 +#define MAVLINK_MSG_ID_110_LEN 254 + +#define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC 84 +#define MAVLINK_MSG_ID_110_CRC 84 + +#define MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN 251 + +#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL { \ + "FILE_TRANSFER_PROTOCOL", \ + 4, \ + { { "target_network", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_file_transfer_protocol_t, target_network) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_file_transfer_protocol_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_file_transfer_protocol_t, target_component) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 251, 3, offsetof(mavlink_file_transfer_protocol_t, payload) }, \ + } \ +} + + +/** + * @brief Pack a file_transfer_protocol message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} + +/** + * @brief Pack a file_transfer_protocol message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_network,uint8_t target_system,uint8_t target_component,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} + +/** + * @brief Encode a file_transfer_protocol struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param file_transfer_protocol C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ + return mavlink_msg_file_transfer_protocol_pack(system_id, component_id, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); +} + +/** + * @brief Encode a file_transfer_protocol struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_protocol C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ + return mavlink_msg_file_transfer_protocol_pack_chan(system_id, component_id, chan, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); +} + +/** + * @brief Send a file_transfer_protocol message + * @param chan MAVLink channel to send the message + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_file_transfer_protocol_send(mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_protocol_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#else + mavlink_file_transfer_protocol_t *packet = (mavlink_file_transfer_protocol_t *)msgbuf; + packet->target_network = target_network; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*251); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE FILE_TRANSFER_PROTOCOL UNPACKING + + +/** + * @brief Get field target_network from file_transfer_protocol message + * + * @return Network ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_network(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_system from file_transfer_protocol message + * + * @return System ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field target_component from file_transfer_protocol message + * + * @return Component ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field payload from file_transfer_protocol message + * + * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 251, 3); +} + +/** + * @brief Decode a file_transfer_protocol message into a struct + * + * @param msg The message to decode + * @param file_transfer_protocol C-struct to decode the message contents into + */ +static inline void mavlink_msg_file_transfer_protocol_decode(const mavlink_message_t* msg, mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ +#if MAVLINK_NEED_BYTE_SWAP + file_transfer_protocol->target_network = mavlink_msg_file_transfer_protocol_get_target_network(msg); + file_transfer_protocol->target_system = mavlink_msg_file_transfer_protocol_get_target_system(msg); + file_transfer_protocol->target_component = mavlink_msg_file_transfer_protocol_get_target_component(msg); + mavlink_msg_file_transfer_protocol_get_payload(msg, file_transfer_protocol->payload); +#else + memcpy(file_transfer_protocol, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int.h new file mode 100644 index 0000000..f51b782 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int.h @@ -0,0 +1,401 @@ +// MESSAGE GLOBAL_POSITION_INT PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33 + +typedef struct __mavlink_global_position_int_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well) + int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX +} mavlink_global_position_int_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 +#define MAVLINK_MSG_ID_33_LEN 28 + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC 104 +#define MAVLINK_MSG_ID_33_CRC 104 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ + "GLOBAL_POSITION_INT", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_int_t, time_boot_ms) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_global_position_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_global_position_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_global_position_int_t, vz) }, \ + { "hdg", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_global_position_int_t, hdg) }, \ + } \ +} + + +/** + * @brief Pack a global_position_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well) + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +} + +/** + * @brief Pack a global_position_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well) + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +} + +/** + * @brief Encode a global_position_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + +/** + * @brief Encode a global_position_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + +/** + * @brief Send a global_position_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well) + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#else + mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->hdg = hdg; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GLOBAL_POSITION_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from global_position_int message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field lat from global_position_int message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon from global_position_int message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from global_position_int message + * + * @return Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well) + */ +static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field relative_alt from global_position_int message + * + * @return Altitude above ground in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_global_position_int_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field vx from global_position_int message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field vy from global_position_int message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field vz from global_position_int message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 24); +} + +/** + * @brief Get field hdg from global_position_int message + * + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Decode a global_position_int message into a struct + * + * @param msg The message to decode + * @param global_position_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position_int->time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(msg); + global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); + global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); + global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); + global_position_int->relative_alt = mavlink_msg_global_position_int_get_relative_alt(msg); + global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); + global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); + global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); + global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg); +#else + memcpy(global_position_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int_cov.h b/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int_cov.h new file mode 100644 index 0000000..4d053ba --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_global_position_int_cov.h @@ -0,0 +1,441 @@ +// MESSAGE GLOBAL_POSITION_INT_COV PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV 63 + +typedef struct __mavlink_global_position_int_cov_t +{ + uint64_t time_utc; ///< Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int32_t lat; ///< Latitude, expressed as degrees * 1E7 + int32_t lon; ///< Longitude, expressed as degrees * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL + int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters) + float vx; ///< Ground X Speed (Latitude), expressed as m/s + float vy; ///< Ground Y Speed (Longitude), expressed as m/s + float vz; ///< Ground Z Speed (Altitude), expressed as m/s + float covariance[36]; ///< Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + uint8_t estimator_type; ///< Class id of the estimator this estimate originated from. +} mavlink_global_position_int_cov_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN 185 +#define MAVLINK_MSG_ID_63_LEN 185 + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC 51 +#define MAVLINK_MSG_ID_63_CRC 51 + +#define MAVLINK_MSG_GLOBAL_POSITION_INT_COV_FIELD_COVARIANCE_LEN 36 + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV { \ + "GLOBAL_POSITION_INT_COV", \ + 11, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_int_cov_t, time_utc) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_global_position_int_cov_t, time_boot_ms) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_cov_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_cov_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_global_position_int_cov_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_global_position_int_cov_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_int_cov_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_global_position_int_cov_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_global_position_int_cov_t, vz) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 36, 40, offsetof(mavlink_global_position_int_cov_t, covariance) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 184, offsetof(mavlink_global_position_int_cov_t, estimator_type) }, \ + } \ +} + + +/** + * @brief Pack a global_position_int_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} + +/** + * @brief Pack a global_position_int_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t estimator_type,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,float vx,float vy,float vz,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} + +/** + * @brief Encode a global_position_int_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_position_int_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_cov_t* global_position_int_cov) +{ + return mavlink_msg_global_position_int_cov_pack(system_id, component_id, msg, global_position_int_cov->time_boot_ms, global_position_int_cov->time_utc, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); +} + +/** + * @brief Encode a global_position_int_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_cov_t* global_position_int_cov) +{ + return mavlink_msg_global_position_int_cov_pack_chan(system_id, component_id, chan, msg, global_position_int_cov->time_boot_ms, global_position_int_cov->time_utc, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); +} + +/** + * @brief Send a global_position_int_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat Latitude, expressed as degrees * 1E7 + * @param lon Longitude, expressed as degrees * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL + * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s + * @param vy Ground Y Speed (Longitude), expressed as m/s + * @param vz Ground Z Speed (Altitude), expressed as m/s + * @param covariance Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_int_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#else + mavlink_global_position_int_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint8_t(buf, 184, estimator_type); + _mav_put_float_array(buf, 40, covariance, 36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#else + mavlink_global_position_int_cov_t *packet = (mavlink_global_position_int_cov_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*36); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GLOBAL_POSITION_INT_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from global_position_int_cov message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_global_position_int_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from global_position_int_cov message + * + * @return Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + */ +static inline uint64_t mavlink_msg_global_position_int_cov_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field estimator_type from global_position_int_cov message + * + * @return Class id of the estimator this estimate originated from. + */ +static inline uint8_t mavlink_msg_global_position_int_cov_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 184); +} + +/** + * @brief Get field lat from global_position_int_cov message + * + * @return Latitude, expressed as degrees * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lon from global_position_int_cov message + * + * @return Longitude, expressed as degrees * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field alt from global_position_int_cov message + * + * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field relative_alt from global_position_int_cov message + * + * @return Altitude above ground in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_global_position_int_cov_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field vx from global_position_int_cov message + * + * @return Ground X Speed (Latitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vy from global_position_int_cov message + * + * @return Ground Y Speed (Longitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field vz from global_position_int_cov message + * + * @return Ground Z Speed (Altitude), expressed as m/s + */ +static inline float mavlink_msg_global_position_int_cov_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field covariance from global_position_int_cov message + * + * @return Covariance matrix (first six entries are the first ROW, next six entries are the second row, etc.) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 36, 40); +} + +/** + * @brief Decode a global_position_int_cov message into a struct + * + * @param msg The message to decode + * @param global_position_int_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_int_cov_decode(const mavlink_message_t* msg, mavlink_global_position_int_cov_t* global_position_int_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position_int_cov->time_utc = mavlink_msg_global_position_int_cov_get_time_utc(msg); + global_position_int_cov->time_boot_ms = mavlink_msg_global_position_int_cov_get_time_boot_ms(msg); + global_position_int_cov->lat = mavlink_msg_global_position_int_cov_get_lat(msg); + global_position_int_cov->lon = mavlink_msg_global_position_int_cov_get_lon(msg); + global_position_int_cov->alt = mavlink_msg_global_position_int_cov_get_alt(msg); + global_position_int_cov->relative_alt = mavlink_msg_global_position_int_cov_get_relative_alt(msg); + global_position_int_cov->vx = mavlink_msg_global_position_int_cov_get_vx(msg); + global_position_int_cov->vy = mavlink_msg_global_position_int_cov_get_vy(msg); + global_position_int_cov->vz = mavlink_msg_global_position_int_cov_get_vz(msg); + mavlink_msg_global_position_int_cov_get_covariance(msg, global_position_int_cov->covariance); + global_position_int_cov->estimator_type = mavlink_msg_global_position_int_cov_get_estimator_type(msg); +#else + memcpy(global_position_int_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_global_vision_position_estimate.h b/flightcode/mavlink/c_library/common/mavlink_msg_global_vision_position_estimate.h new file mode 100644 index 0000000..b2b75d7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_global_vision_position_estimate.h @@ -0,0 +1,353 @@ +// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE PACKING + +#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE 101 + +typedef struct __mavlink_global_vision_position_estimate_t +{ + uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad +} mavlink_global_vision_position_estimate_t; + +#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_101_LEN 32 + +#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC 102 +#define MAVLINK_MSG_ID_101_CRC 102 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ + "GLOBAL_VISION_POSITION_ESTIMATE", \ + 7, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_vision_position_estimate_t, z) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a global_vision_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_global_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Pack a global_vision_position_estimate message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_global_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Encode a global_vision_position_estimate struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); +} + +/** + * @brief Encode a global_vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); +} + +/** + * @brief Send a global_vision_position_estimate message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_global_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING + + +/** + * @brief Get field usec from global_vision_position_estimate message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_global_vision_position_estimate_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field x from global_vision_position_estimate message + * + * @return Global X position + */ +static inline float mavlink_msg_global_vision_position_estimate_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from global_vision_position_estimate message + * + * @return Global Y position + */ +static inline float mavlink_msg_global_vision_position_estimate_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from global_vision_position_estimate message + * + * @return Global Z position + */ +static inline float mavlink_msg_global_vision_position_estimate_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field roll from global_vision_position_estimate message + * + * @return Roll angle in rad + */ +static inline float mavlink_msg_global_vision_position_estimate_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitch from global_vision_position_estimate message + * + * @return Pitch angle in rad + */ +static inline float mavlink_msg_global_vision_position_estimate_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw from global_vision_position_estimate message + * + * @return Yaw angle in rad + */ +static inline float mavlink_msg_global_vision_position_estimate_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a global_vision_position_estimate message into a struct + * + * @param msg The message to decode + * @param global_vision_position_estimate C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_vision_position_estimate->usec = mavlink_msg_global_vision_position_estimate_get_usec(msg); + global_vision_position_estimate->x = mavlink_msg_global_vision_position_estimate_get_x(msg); + global_vision_position_estimate->y = mavlink_msg_global_vision_position_estimate_get_y(msg); + global_vision_position_estimate->z = mavlink_msg_global_vision_position_estimate_get_z(msg); + global_vision_position_estimate->roll = mavlink_msg_global_vision_position_estimate_get_roll(msg); + global_vision_position_estimate->pitch = mavlink_msg_global_vision_position_estimate_get_pitch(msg); + global_vision_position_estimate->yaw = mavlink_msg_global_vision_position_estimate_get_yaw(msg); +#else + memcpy(global_vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps2_raw.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps2_raw.h new file mode 100644 index 0000000..9101b24 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps2_raw.h @@ -0,0 +1,473 @@ +// MESSAGE GPS2_RAW PACKING + +#define MAVLINK_MSG_ID_GPS2_RAW 124 + +typedef struct __mavlink_gps2_raw_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + uint32_t dgps_age; ///< Age of DGPS info + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 + uint8_t dgps_numch; ///< Number of DGPS satellites +} mavlink_gps2_raw_t; + +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 +#define MAVLINK_MSG_ID_124_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 +#define MAVLINK_MSG_ID_124_CRC 87 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ + "GPS2_RAW", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \ + { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ + { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ + } \ +} + + +/** + * @brief Pack a gps2_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Pack a gps2_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Encode a gps2_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Encode a gps2_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Send a gps2_raw message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->dgps_age = dgps_age; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + packet->dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS2_RAW UNPACKING + + +/** + * @brief Get field time_usec from gps2_raw message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps2_raw message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field lat from gps2_raw message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from gps2_raw message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from gps2_raw message + * + * @return Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from gps2_raw message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field epv from gps2_raw message + * + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field vel from gps2_raw message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field cog from gps2_raw message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field satellites_visible from gps2_raw message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field dgps_numch from gps2_raw message + * + * @return Number of DGPS satellites + */ +static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field dgps_age from gps2_raw message + * + * @return Age of DGPS info + */ +static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Decode a gps2_raw message into a struct + * + * @param msg The message to decode + * @param gps2_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg); + gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg); + gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg); + gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg); + gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg); + gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg); + gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg); + gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg); + gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg); + gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); + gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); + gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); +#else + memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps2_rtk.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps2_rtk.h new file mode 100644 index 0000000..4c2d93c --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps2_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS2_RTK PACKING + +#define MAVLINK_MSG_ID_GPS2_RTK 128 + +typedef struct __mavlink_gps2_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps2_rtk_t; + +#define MAVLINK_MSG_ID_GPS2_RTK_LEN 35 +#define MAVLINK_MSG_ID_128_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RTK_CRC 226 +#define MAVLINK_MSG_ID_128_CRC 226 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RTK { \ + "GPS2_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps2_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps2_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps2_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps2_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps2_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps2_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps2_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps2_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack(system_id, component_id, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps2_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack_chan(system_id, component_id, chan, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps2_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS2_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t *packet = (mavlink_gps2_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS2_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps2_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps2_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps2_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps2_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps2_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps2_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps2_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps2_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps2_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps2_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps2_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps2_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps2_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps2_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps2_rtk message into a struct + * + * @param msg The message to decode + * @param gps2_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_rtk_decode(const mavlink_message_t* msg, mavlink_gps2_rtk_t* gps2_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_rtk->time_last_baseline_ms = mavlink_msg_gps2_rtk_get_time_last_baseline_ms(msg); + gps2_rtk->tow = mavlink_msg_gps2_rtk_get_tow(msg); + gps2_rtk->baseline_a_mm = mavlink_msg_gps2_rtk_get_baseline_a_mm(msg); + gps2_rtk->baseline_b_mm = mavlink_msg_gps2_rtk_get_baseline_b_mm(msg); + gps2_rtk->baseline_c_mm = mavlink_msg_gps2_rtk_get_baseline_c_mm(msg); + gps2_rtk->accuracy = mavlink_msg_gps2_rtk_get_accuracy(msg); + gps2_rtk->iar_num_hypotheses = mavlink_msg_gps2_rtk_get_iar_num_hypotheses(msg); + gps2_rtk->wn = mavlink_msg_gps2_rtk_get_wn(msg); + gps2_rtk->rtk_receiver_id = mavlink_msg_gps2_rtk_get_rtk_receiver_id(msg); + gps2_rtk->rtk_health = mavlink_msg_gps2_rtk_get_rtk_health(msg); + gps2_rtk->rtk_rate = mavlink_msg_gps2_rtk_get_rtk_rate(msg); + gps2_rtk->nsats = mavlink_msg_gps2_rtk_get_nsats(msg); + gps2_rtk->baseline_coords_type = mavlink_msg_gps2_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps2_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps_global_origin.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps_global_origin.h new file mode 100644 index 0000000..5ae4c9b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps_global_origin.h @@ -0,0 +1,257 @@ +// MESSAGE GPS_GLOBAL_ORIGIN PACKING + +#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49 + +typedef struct __mavlink_gps_global_origin_t +{ + int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 + int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 + int32_t altitude; ///< Altitude (AMSL), in meters * 1000 (positive for up) +} mavlink_gps_global_origin_t; + +#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 12 +#define MAVLINK_MSG_ID_49_LEN 12 + +#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC 39 +#define MAVLINK_MSG_ID_49_CRC 39 + + + +#define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ + "GPS_GLOBAL_ORIGIN", \ + 3, \ + { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ + { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ + } \ +} + + +/** + * @brief Pack a gps_global_origin message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84), in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + +/** + * @brief Pack a gps_global_origin message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84), in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t latitude,int32_t longitude,int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + +/** + * @brief Encode a gps_global_origin struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); +} + +/** + * @brief Encode a gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); +} + +/** + * @brief Send a gps_global_origin message + * @param chan MAVLink channel to send the message + * + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84), in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_GLOBAL_ORIGIN UNPACKING + + +/** + * @brief Get field latitude from gps_global_origin message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field longitude from gps_global_origin message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field altitude from gps_global_origin message + * + * @return Altitude (AMSL), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Decode a gps_global_origin message into a struct + * + * @param msg The message to decode + * @param gps_global_origin C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_global_origin_t* gps_global_origin) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_global_origin->latitude = mavlink_msg_gps_global_origin_get_latitude(msg); + gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg); + gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg); +#else + memcpy(gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps_inject_data.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps_inject_data.h new file mode 100644 index 0000000..362e2d7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps_inject_data.h @@ -0,0 +1,273 @@ +// MESSAGE GPS_INJECT_DATA PACKING + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123 + +typedef struct __mavlink_gps_inject_data_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t len; ///< data length + uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2) +} mavlink_gps_inject_data_t; + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113 +#define MAVLINK_MSG_ID_123_LEN 113 + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250 +#define MAVLINK_MSG_ID_123_CRC 250 + +#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110 + +#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \ + "GPS_INJECT_DATA", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a gps_inject_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Pack a gps_inject_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Encode a gps_inject_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Encode a gps_inject_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Send a gps_inject_data message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_INJECT_DATA UNPACKING + + +/** + * @brief Get field target_system from gps_inject_data message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from gps_inject_data message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field len from gps_inject_data message + * + * @return data length + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field data from gps_inject_data message + * + * @return raw data (110 is enough for 12 satellites of RTCMv2) + */ +static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 110, 3); +} + +/** + * @brief Decode a gps_inject_data message into a struct + * + * @param msg The message to decode + * @param gps_inject_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg); + gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg); + gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg); + mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data); +#else + memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps_raw_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps_raw_int.h new file mode 100644 index 0000000..b66171d --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps_raw_int.h @@ -0,0 +1,425 @@ +// MESSAGE GPS_RAW_INT PACKING + +#define MAVLINK_MSG_ID_GPS_RAW_INT 24 + +typedef struct __mavlink_gps_raw_int_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 +} mavlink_gps_raw_int_t; + +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_ID_24_LEN 30 + +#define MAVLINK_MSG_ID_GPS_RAW_INT_CRC 24 +#define MAVLINK_MSG_ID_24_CRC 24 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ + "GPS_RAW_INT", \ + 10, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_raw_int_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_raw_int_t, alt) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_gps_raw_int_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_gps_raw_int_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + } \ +} + + +/** + * @brief Pack a gps_raw_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +} + +/** + * @brief Pack a gps_raw_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +} + +/** + * @brief Encode a gps_raw_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); +} + +/** + * @brief Encode a gps_raw_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); +} + +/** + * @brief Send a gps_raw_int message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#else + mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_RAW_INT UNPACKING + + +/** + * @brief Get field time_usec from gps_raw_int message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps_raw_int message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field lat from gps_raw_int message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from gps_raw_int message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from gps_raw_int message + * + * @return Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. + */ +static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from gps_raw_int message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field epv from gps_raw_int message + * + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field vel from gps_raw_int message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field cog from gps_raw_int message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field satellites_visible from gps_raw_int message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Decode a gps_raw_int message into a struct + * + * @param msg The message to decode + * @param gps_raw_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_raw_int->time_usec = mavlink_msg_gps_raw_int_get_time_usec(msg); + gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); + gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); + gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); + gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); + gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); + gps_raw_int->vel = mavlink_msg_gps_raw_int_get_vel(msg); + gps_raw_int->cog = mavlink_msg_gps_raw_int_get_cog(msg); + gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); + gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); +#else + memcpy(gps_raw_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps_rtk.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps_rtk.h new file mode 100644 index 0000000..31052ec --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS_RTK PACKING + +#define MAVLINK_MSG_ID_GPS_RTK 127 + +typedef struct __mavlink_gps_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps_rtk_t; + +#define MAVLINK_MSG_ID_GPS_RTK_LEN 35 +#define MAVLINK_MSG_ID_127_LEN 35 + +#define MAVLINK_MSG_ID_GPS_RTK_CRC 25 +#define MAVLINK_MSG_ID_127_CRC 25 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RTK { \ + "GPS_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack(system_id, component_id, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack_chan(system_id, component_id, chan, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t *packet = (mavlink_gps_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps_rtk message into a struct + * + * @param msg The message to decode + * @param gps_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_rtk_decode(const mavlink_message_t* msg, mavlink_gps_rtk_t* gps_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_rtk->time_last_baseline_ms = mavlink_msg_gps_rtk_get_time_last_baseline_ms(msg); + gps_rtk->tow = mavlink_msg_gps_rtk_get_tow(msg); + gps_rtk->baseline_a_mm = mavlink_msg_gps_rtk_get_baseline_a_mm(msg); + gps_rtk->baseline_b_mm = mavlink_msg_gps_rtk_get_baseline_b_mm(msg); + gps_rtk->baseline_c_mm = mavlink_msg_gps_rtk_get_baseline_c_mm(msg); + gps_rtk->accuracy = mavlink_msg_gps_rtk_get_accuracy(msg); + gps_rtk->iar_num_hypotheses = mavlink_msg_gps_rtk_get_iar_num_hypotheses(msg); + gps_rtk->wn = mavlink_msg_gps_rtk_get_wn(msg); + gps_rtk->rtk_receiver_id = mavlink_msg_gps_rtk_get_rtk_receiver_id(msg); + gps_rtk->rtk_health = mavlink_msg_gps_rtk_get_rtk_health(msg); + gps_rtk->rtk_rate = mavlink_msg_gps_rtk_get_rtk_rate(msg); + gps_rtk->nsats = mavlink_msg_gps_rtk_get_nsats(msg); + gps_rtk->baseline_coords_type = mavlink_msg_gps_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_gps_status.h b/flightcode/mavlink/c_library/common/mavlink_msg_gps_status.h new file mode 100644 index 0000000..10659d0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_gps_status.h @@ -0,0 +1,325 @@ +// MESSAGE GPS_STATUS PACKING + +#define MAVLINK_MSG_ID_GPS_STATUS 25 + +typedef struct __mavlink_gps_status_t +{ + uint8_t satellites_visible; ///< Number of satellites visible + uint8_t satellite_prn[20]; ///< Global satellite ID + uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite +} mavlink_gps_status_t; + +#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_ID_25_LEN 101 + +#define MAVLINK_MSG_ID_GPS_STATUS_CRC 23 +#define MAVLINK_MSG_ID_25_CRC 23 + +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20 + +#define MAVLINK_MESSAGE_INFO_GPS_STATUS { \ + "GPS_STATUS", \ + 6, \ + { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \ + { "satellite_prn", NULL, MAVLINK_TYPE_UINT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \ + { "satellite_used", NULL, MAVLINK_TYPE_UINT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \ + { "satellite_elevation", NULL, MAVLINK_TYPE_UINT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \ + { "satellite_azimuth", NULL, MAVLINK_TYPE_UINT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \ + { "satellite_snr", NULL, MAVLINK_TYPE_UINT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \ + } \ +} + + +/** + * @brief Pack a gps_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +} + +/** + * @brief Pack a gps_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t satellites_visible,const uint8_t *satellite_prn,const uint8_t *satellite_used,const uint8_t *satellite_elevation,const uint8_t *satellite_azimuth,const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +} + +/** + * @brief Encode a gps_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + +/** + * @brief Encode a gps_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + +/** + * @brief Send a gps_status message + * @param chan MAVLink channel to send the message + * + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#else + mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; + packet->satellites_visible = satellites_visible; + mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_STATUS UNPACKING + + +/** + * @brief Get field satellites_visible from gps_status message + * + * @return Number of satellites visible + */ +static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field satellite_prn from gps_status message + * + * @return Global satellite ID + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t *satellite_prn) +{ + return _MAV_RETURN_uint8_t_array(msg, satellite_prn, 20, 1); +} + +/** + * @brief Get field satellite_used from gps_status message + * + * @return 0: Satellite not used, 1: used for localization + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t *satellite_used) +{ + return _MAV_RETURN_uint8_t_array(msg, satellite_used, 20, 21); +} + +/** + * @brief Get field satellite_elevation from gps_status message + * + * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t *satellite_elevation) +{ + return _MAV_RETURN_uint8_t_array(msg, satellite_elevation, 20, 41); +} + +/** + * @brief Get field satellite_azimuth from gps_status message + * + * @return Direction of satellite, 0: 0 deg, 255: 360 deg. + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t *satellite_azimuth) +{ + return _MAV_RETURN_uint8_t_array(msg, satellite_azimuth, 20, 61); +} + +/** + * @brief Get field satellite_snr from gps_status message + * + * @return Signal to noise ratio of satellite + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t *satellite_snr) +{ + return _MAV_RETURN_uint8_t_array(msg, satellite_snr, 20, 81); +} + +/** + * @brief Decode a gps_status message into a struct + * + * @param msg The message to decode + * @param gps_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg); + mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn); + mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used); + mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation); + mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); + mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); +#else + memcpy(gps_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_heartbeat.h b/flightcode/mavlink/c_library/common/mavlink_msg_heartbeat.h new file mode 100644 index 0000000..733886b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_heartbeat.h @@ -0,0 +1,326 @@ +// MESSAGE HEARTBEAT PACKING + +#define MAVLINK_MSG_ID_HEARTBEAT 0 + +typedef struct __mavlink_heartbeat_t +{ + uint32_t custom_mode; ///< A bitfield for use for autopilot-specific flags. + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Autopilot type / class. defined in MAV_AUTOPILOT ENUM + uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + uint8_t system_status; ///< System status flag, see MAV_STATE ENUM + uint8_t mavlink_version; ///< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version +} mavlink_heartbeat_t; + +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 +#define MAVLINK_MSG_ID_0_LEN 9 + +#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 +#define MAVLINK_MSG_ID_0_CRC 50 + + + +#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ + "HEARTBEAT", \ + 6, \ + { { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, base_mode) }, \ + { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, system_status) }, \ + { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} + + +/** + * @brief Pack a heartbeat message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} + +/** + * @brief Pack a heartbeat message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} + +/** + * @brief Encode a heartbeat struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** + * @brief Encode a heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** + * @brief Send a heartbeat message + * @param chan MAVLink channel to send the message + * + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#else + mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->type = type; + packet->autopilot = autopilot; + packet->base_mode = base_mode; + packet->system_status = system_status; + packet->mavlink_version = 3; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HEARTBEAT UNPACKING + + +/** + * @brief Get field type from heartbeat message + * + * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + */ +static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field autopilot from heartbeat message + * + * @return Autopilot type / class. defined in MAV_AUTOPILOT ENUM + */ +static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field base_mode from heartbeat message + * + * @return System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + */ +static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field custom_mode from heartbeat message + * + * @return A bitfield for use for autopilot-specific flags. + */ +static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field system_status from heartbeat message + * + * @return System status flag, see MAV_STATE ENUM + */ +static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field mavlink_version from heartbeat message + * + * @return MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + */ +static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Decode a heartbeat message into a struct + * + * @param msg The message to decode + * @param heartbeat C-struct to decode the message contents into + */ +static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) +{ +#if MAVLINK_NEED_BYTE_SWAP + heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg); + heartbeat->type = mavlink_msg_heartbeat_get_type(msg); + heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); + heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg); + heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); + heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); +#else + memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_highres_imu.h b/flightcode/mavlink/c_library/common/mavlink_msg_highres_imu.h new file mode 100644 index 0000000..2749cb0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_highres_imu.h @@ -0,0 +1,545 @@ +// MESSAGE HIGHRES_IMU PACKING + +#define MAVLINK_MSG_ID_HIGHRES_IMU 105 + +typedef struct __mavlink_highres_imu_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float xacc; ///< X acceleration (m/s^2) + float yacc; ///< Y acceleration (m/s^2) + float zacc; ///< Z acceleration (m/s^2) + float xgyro; ///< Angular speed around X axis (rad / sec) + float ygyro; ///< Angular speed around Y axis (rad / sec) + float zgyro; ///< Angular speed around Z axis (rad / sec) + float xmag; ///< X Magnetic field (Gauss) + float ymag; ///< Y Magnetic field (Gauss) + float zmag; ///< Z Magnetic field (Gauss) + float abs_pressure; ///< Absolute pressure in millibar + float diff_pressure; ///< Differential pressure in millibar + float pressure_alt; ///< Altitude calculated from pressure + float temperature; ///< Temperature in degrees celsius + uint16_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature +} mavlink_highres_imu_t; + +#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 62 +#define MAVLINK_MSG_ID_105_LEN 62 + +#define MAVLINK_MSG_ID_HIGHRES_IMU_CRC 93 +#define MAVLINK_MSG_ID_105_CRC 93 + + + +#define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ + "HIGHRES_IMU", \ + 15, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_highres_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_highres_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_highres_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_highres_imu_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_highres_imu_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_highres_imu_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_highres_imu_t, zmag) }, \ + { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_highres_imu_t, abs_pressure) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_highres_imu_t, diff_pressure) }, \ + { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ + { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ + } \ +} + + +/** + * @brief Pack a highres_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis (rad / sec) + * @param ygyro Angular speed around Y axis (rad / sec) + * @param zgyro Angular speed around Z axis (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#else + mavlink_highres_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +} + +/** + * @brief Pack a highres_imu message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis (rad / sec) + * @param ygyro Angular speed around Y axis (rad / sec) + * @param zgyro Angular speed around Z axis (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#else + mavlink_highres_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +} + +/** + * @brief Encode a highres_imu struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); +} + +/** + * @brief Encode a highres_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); +} + +/** + * @brief Send a highres_imu message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis (rad / sec) + * @param ygyro Angular speed around Y axis (rad / sec) + * @param zgyro Angular speed around Z axis (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#else + mavlink_highres_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#else + mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIGHRES_IMU UNPACKING + + +/** + * @brief Get field time_usec from highres_imu message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_highres_imu_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from highres_imu message + * + * @return X acceleration (m/s^2) + */ +static inline float mavlink_msg_highres_imu_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yacc from highres_imu message + * + * @return Y acceleration (m/s^2) + */ +static inline float mavlink_msg_highres_imu_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field zacc from highres_imu message + * + * @return Z acceleration (m/s^2) + */ +static inline float mavlink_msg_highres_imu_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field xgyro from highres_imu message + * + * @return Angular speed around X axis (rad / sec) + */ +static inline float mavlink_msg_highres_imu_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field ygyro from highres_imu message + * + * @return Angular speed around Y axis (rad / sec) + */ +static inline float mavlink_msg_highres_imu_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field zgyro from highres_imu message + * + * @return Angular speed around Z axis (rad / sec) + */ +static inline float mavlink_msg_highres_imu_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field xmag from highres_imu message + * + * @return X Magnetic field (Gauss) + */ +static inline float mavlink_msg_highres_imu_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field ymag from highres_imu message + * + * @return Y Magnetic field (Gauss) + */ +static inline float mavlink_msg_highres_imu_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field zmag from highres_imu message + * + * @return Z Magnetic field (Gauss) + */ +static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field abs_pressure from highres_imu message + * + * @return Absolute pressure in millibar + */ +static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field diff_pressure from highres_imu message + * + * @return Differential pressure in millibar + */ +static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field pressure_alt from highres_imu message + * + * @return Altitude calculated from pressure + */ +static inline float mavlink_msg_highres_imu_get_pressure_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field temperature from highres_imu message + * + * @return Temperature in degrees celsius + */ +static inline float mavlink_msg_highres_imu_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field fields_updated from highres_imu message + * + * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 60); +} + +/** + * @brief Decode a highres_imu message into a struct + * + * @param msg The message to decode + * @param highres_imu C-struct to decode the message contents into + */ +static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, mavlink_highres_imu_t* highres_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP + highres_imu->time_usec = mavlink_msg_highres_imu_get_time_usec(msg); + highres_imu->xacc = mavlink_msg_highres_imu_get_xacc(msg); + highres_imu->yacc = mavlink_msg_highres_imu_get_yacc(msg); + highres_imu->zacc = mavlink_msg_highres_imu_get_zacc(msg); + highres_imu->xgyro = mavlink_msg_highres_imu_get_xgyro(msg); + highres_imu->ygyro = mavlink_msg_highres_imu_get_ygyro(msg); + highres_imu->zgyro = mavlink_msg_highres_imu_get_zgyro(msg); + highres_imu->xmag = mavlink_msg_highres_imu_get_xmag(msg); + highres_imu->ymag = mavlink_msg_highres_imu_get_ymag(msg); + highres_imu->zmag = mavlink_msg_highres_imu_get_zmag(msg); + highres_imu->abs_pressure = mavlink_msg_highres_imu_get_abs_pressure(msg); + highres_imu->diff_pressure = mavlink_msg_highres_imu_get_diff_pressure(msg); + highres_imu->pressure_alt = mavlink_msg_highres_imu_get_pressure_alt(msg); + highres_imu->temperature = mavlink_msg_highres_imu_get_temperature(msg); + highres_imu->fields_updated = mavlink_msg_highres_imu_get_fields_updated(msg); +#else + memcpy(highres_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_controls.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_controls.h new file mode 100644 index 0000000..f7507b1 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_controls.h @@ -0,0 +1,449 @@ +// MESSAGE HIL_CONTROLS PACKING + +#define MAVLINK_MSG_ID_HIL_CONTROLS 91 + +typedef struct __mavlink_hil_controls_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll_ailerons; ///< Control output -1 .. 1 + float pitch_elevator; ///< Control output -1 .. 1 + float yaw_rudder; ///< Control output -1 .. 1 + float throttle; ///< Throttle 0 .. 1 + float aux1; ///< Aux 1, -1 .. 1 + float aux2; ///< Aux 2, -1 .. 1 + float aux3; ///< Aux 3, -1 .. 1 + float aux4; ///< Aux 4, -1 .. 1 + uint8_t mode; ///< System mode (MAV_MODE) + uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) +} mavlink_hil_controls_t; + +#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 42 +#define MAVLINK_MSG_ID_91_LEN 42 + +#define MAVLINK_MSG_ID_HIL_CONTROLS_CRC 63 +#define MAVLINK_MSG_ID_91_CRC 63 + + + +#define MAVLINK_MESSAGE_INFO_HIL_CONTROLS { \ + "HIL_CONTROLS", \ + 11, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_usec) }, \ + { "roll_ailerons", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \ + { "pitch_elevator", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \ + { "yaw_rudder", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \ + { "throttle", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \ + { "aux1", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_controls_t, aux1) }, \ + { "aux2", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_controls_t, aux2) }, \ + { "aux3", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_controls_t, aux3) }, \ + { "aux4", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_controls_t, aux4) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_hil_controls_t, mode) }, \ + { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_hil_controls_t, nav_mode) }, \ + } \ +} + + +/** + * @brief Pack a hil_controls message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -1 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param aux1 Aux 1, -1 .. 1 + * @param aux2 Aux 2, -1 .. 1 + * @param aux3 Aux 3, -1 .. 1 + * @param aux4 Aux 4, -1 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +} + +/** + * @brief Pack a hil_controls message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -1 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param aux1 Aux 1, -1 .. 1 + * @param aux2 Aux 2, -1 .. 1 + * @param aux3 Aux 3, -1 .. 1 + * @param aux4 Aux 4, -1 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,float aux1,float aux2,float aux3,float aux4,uint8_t mode,uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +} + +/** + * @brief Encode a hil_controls struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + +/** + * @brief Encode a hil_controls struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + +/** + * @brief Send a hil_controls message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -1 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param aux1 Aux 1, -1 .. 1 + * @param aux2 Aux 2, -1 .. 1 + * @param aux3 Aux 3, -1 .. 1 + * @param aux4 Aux 4, -1 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#else + mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll_ailerons = roll_ailerons; + packet->pitch_elevator = pitch_elevator; + packet->yaw_rudder = yaw_rudder; + packet->throttle = throttle; + packet->aux1 = aux1; + packet->aux2 = aux2; + packet->aux3 = aux3; + packet->aux4 = aux4; + packet->mode = mode; + packet->nav_mode = nav_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_CONTROLS UNPACKING + + +/** + * @brief Get field time_usec from hil_controls message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_controls_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll_ailerons from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_elevator from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rudder from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field throttle from hil_controls message + * + * @return Throttle 0 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field aux1 from hil_controls message + * + * @return Aux 1, -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field aux2 from hil_controls message + * + * @return Aux 2, -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field aux3 from hil_controls message + * + * @return Aux 3, -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field aux4 from hil_controls message + * + * @return Aux 4, -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field mode from hil_controls message + * + * @return System mode (MAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field nav_mode from hil_controls message + * + * @return Navigation mode (MAV_NAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Decode a hil_controls message into a struct + * + * @param msg The message to decode + * @param hil_controls C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_controls->time_usec = mavlink_msg_hil_controls_get_time_usec(msg); + hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg); + hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg); + hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg); + hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg); + hil_controls->aux1 = mavlink_msg_hil_controls_get_aux1(msg); + hil_controls->aux2 = mavlink_msg_hil_controls_get_aux2(msg); + hil_controls->aux3 = mavlink_msg_hil_controls_get_aux3(msg); + hil_controls->aux4 = mavlink_msg_hil_controls_get_aux4(msg); + hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); + hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); +#else + memcpy(hil_controls, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_gps.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_gps.h new file mode 100644 index 0000000..c140a70 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_gps.h @@ -0,0 +1,497 @@ +// MESSAGE HIL_GPS PACKING + +#define MAVLINK_MSG_ID_HIL_GPS 113 + +typedef struct __mavlink_hil_gps_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 + int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame + int16_t vd; ///< GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 +} mavlink_hil_gps_t; + +#define MAVLINK_MSG_ID_HIL_GPS_LEN 36 +#define MAVLINK_MSG_ID_113_LEN 36 + +#define MAVLINK_MSG_ID_HIL_GPS_CRC 124 +#define MAVLINK_MSG_ID_113_CRC 124 + + + +#define MAVLINK_MESSAGE_INFO_HIL_GPS { \ + "HIL_GPS", \ + 13, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_hil_gps_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_hil_gps_t, alt) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_gps_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_gps_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_gps_t, vel) }, \ + { "vn", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_hil_gps_t, vn) }, \ + { "ve", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_hil_gps_t, ve) }, \ + { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + } \ +} + + +/** + * @brief Pack a hil_gps message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_GPS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} + +/** + * @brief Pack a hil_gps message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_GPS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} + +/** + * @brief Encode a hil_gps struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + +/** + * @brief Encode a hil_gps struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + +/** + * @brief Send a hil_gps message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_GPS UNPACKING + + +/** + * @brief Get field time_usec from hil_gps message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_gps_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from hil_gps message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_hil_gps_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field lat from hil_gps message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_hil_gps_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from hil_gps message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_hil_gps_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from hil_gps message + * + * @return Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from hil_gps message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field epv from hil_gps message + * + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field vel from hil_gps message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field vn from hil_gps message + * + * @return GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_vn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 26); +} + +/** + * @brief Get field ve from hil_gps message + * + * @return GPS velocity in cm/s in EAST direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_ve(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 28); +} + +/** + * @brief Get field vd from hil_gps message + * + * @return GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_vd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 30); +} + +/** + * @brief Get field cog from hil_gps message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field satellites_visible from hil_gps message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Decode a hil_gps message into a struct + * + * @param msg The message to decode + * @param hil_gps C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavlink_hil_gps_t* hil_gps) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_gps->time_usec = mavlink_msg_hil_gps_get_time_usec(msg); + hil_gps->lat = mavlink_msg_hil_gps_get_lat(msg); + hil_gps->lon = mavlink_msg_hil_gps_get_lon(msg); + hil_gps->alt = mavlink_msg_hil_gps_get_alt(msg); + hil_gps->eph = mavlink_msg_hil_gps_get_eph(msg); + hil_gps->epv = mavlink_msg_hil_gps_get_epv(msg); + hil_gps->vel = mavlink_msg_hil_gps_get_vel(msg); + hil_gps->vn = mavlink_msg_hil_gps_get_vn(msg); + hil_gps->ve = mavlink_msg_hil_gps_get_ve(msg); + hil_gps->vd = mavlink_msg_hil_gps_get_vd(msg); + hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg); + hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg); + hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg); +#else + memcpy(hil_gps, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_optical_flow.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_optical_flow.h new file mode 100644 index 0000000..7acb0b8 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_optical_flow.h @@ -0,0 +1,473 @@ +// MESSAGE HIL_OPTICAL_FLOW PACKING + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW 114 + +typedef struct __mavlink_hil_optical_flow_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float integrated_xgyro; ///< RH rotation around X axis (rad) + float integrated_ygyro; ///< RH rotation around Y axis (rad) + float integrated_zgyro; ///< RH rotation around Z axis (rad) + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality +} mavlink_hil_optical_flow_t; + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 44 +#define MAVLINK_MSG_ID_114_LEN 44 + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 237 +#define MAVLINK_MSG_ID_114_CRC 237 + + + +#define MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW { \ + "HIL_OPTICAL_FLOW", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_optical_flow_t, time_usec) }, \ + { "integration_time_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_hil_optical_flow_t, integration_time_us) }, \ + { "integrated_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, integrated_x) }, \ + { "integrated_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, integrated_y) }, \ + { "integrated_xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_optical_flow_t, integrated_xgyro) }, \ + { "integrated_ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_optical_flow_t, integrated_ygyro) }, \ + { "integrated_zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_optical_flow_t, integrated_zgyro) }, \ + { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_hil_optical_flow_t, time_delta_distance_us) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_optical_flow_t, distance) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_hil_optical_flow_t, temperature) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_hil_optical_flow_t, quality) }, \ + } \ +} + + +/** + * @brief Pack a hil_optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Pack a hil_optical_flow message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,float integrated_xgyro,float integrated_ygyro,float integrated_zgyro,int16_t temperature,uint8_t quality,uint32_t time_delta_distance_us,float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Encode a hil_optical_flow struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); +} + +/** + * @brief Encode a hil_optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); +} + +/** + * @brief Send a hil_optical_flow message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->integration_time_us = integration_time_us; + packet->integrated_x = integrated_x; + packet->integrated_y = integrated_y; + packet->integrated_xgyro = integrated_xgyro; + packet->integrated_ygyro = integrated_ygyro; + packet->integrated_zgyro = integrated_zgyro; + packet->time_delta_distance_us = time_delta_distance_us; + packet->distance = distance; + packet->temperature = temperature; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_OPTICAL_FLOW UNPACKING + + +/** + * @brief Get field time_usec from hil_optical_flow message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from hil_optical_flow message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_hil_optical_flow_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field integration_time_us from hil_optical_flow message + * + * @return Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + */ +static inline uint32_t mavlink_msg_hil_optical_flow_get_integration_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field integrated_x from hil_optical_flow message + * + * @return Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field integrated_y from hil_optical_flow message + * + * @return Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field integrated_xgyro from hil_optical_flow message + * + * @return RH rotation around X axis (rad) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field integrated_ygyro from hil_optical_flow message + * + * @return RH rotation around Y axis (rad) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field integrated_zgyro from hil_optical_flow message + * + * @return RH rotation around Z axis (rad) + */ +static inline float mavlink_msg_hil_optical_flow_get_integrated_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field temperature from hil_optical_flow message + * + * @return Temperature * 100 in centi-degrees Celsius + */ +static inline int16_t mavlink_msg_hil_optical_flow_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field quality from hil_optical_flow message + * + * @return Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + */ +static inline uint8_t mavlink_msg_hil_optical_flow_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field time_delta_distance_us from hil_optical_flow message + * + * @return Time in microseconds since the distance was sampled. + */ +static inline uint32_t mavlink_msg_hil_optical_flow_get_time_delta_distance_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 32); +} + +/** + * @brief Get field distance from hil_optical_flow message + * + * @return Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +static inline float mavlink_msg_hil_optical_flow_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a hil_optical_flow message into a struct + * + * @param msg The message to decode + * @param hil_optical_flow C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_optical_flow_decode(const mavlink_message_t* msg, mavlink_hil_optical_flow_t* hil_optical_flow) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_optical_flow->time_usec = mavlink_msg_hil_optical_flow_get_time_usec(msg); + hil_optical_flow->integration_time_us = mavlink_msg_hil_optical_flow_get_integration_time_us(msg); + hil_optical_flow->integrated_x = mavlink_msg_hil_optical_flow_get_integrated_x(msg); + hil_optical_flow->integrated_y = mavlink_msg_hil_optical_flow_get_integrated_y(msg); + hil_optical_flow->integrated_xgyro = mavlink_msg_hil_optical_flow_get_integrated_xgyro(msg); + hil_optical_flow->integrated_ygyro = mavlink_msg_hil_optical_flow_get_integrated_ygyro(msg); + hil_optical_flow->integrated_zgyro = mavlink_msg_hil_optical_flow_get_integrated_zgyro(msg); + hil_optical_flow->time_delta_distance_us = mavlink_msg_hil_optical_flow_get_time_delta_distance_us(msg); + hil_optical_flow->distance = mavlink_msg_hil_optical_flow_get_distance(msg); + hil_optical_flow->temperature = mavlink_msg_hil_optical_flow_get_temperature(msg); + hil_optical_flow->sensor_id = mavlink_msg_hil_optical_flow_get_sensor_id(msg); + hil_optical_flow->quality = mavlink_msg_hil_optical_flow_get_quality(msg); +#else + memcpy(hil_optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_rc_inputs_raw.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_rc_inputs_raw.h new file mode 100644 index 0000000..227cd9d --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_rc_inputs_raw.h @@ -0,0 +1,521 @@ +// MESSAGE HIL_RC_INPUTS_RAW PACKING + +#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW 92 + +typedef struct __mavlink_hil_rc_inputs_raw_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint16_t chan9_raw; ///< RC channel 9 value, in microseconds + uint16_t chan10_raw; ///< RC channel 10 value, in microseconds + uint16_t chan11_raw; ///< RC channel 11 value, in microseconds + uint16_t chan12_raw; ///< RC channel 12 value, in microseconds + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% +} mavlink_hil_rc_inputs_raw_t; + +#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33 +#define MAVLINK_MSG_ID_92_LEN 33 + +#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC 54 +#define MAVLINK_MSG_ID_92_CRC 54 + + + +#define MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW { \ + "HIL_RC_INPUTS_RAW", \ + 14, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_rc_inputs_raw_t, time_usec) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_hil_rc_inputs_raw_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_hil_rc_inputs_raw_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_hil_rc_inputs_raw_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_hil_rc_inputs_raw_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_hil_rc_inputs_raw_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_hil_rc_inputs_raw_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_rc_inputs_raw_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_rc_inputs_raw_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_rc_inputs_raw_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_hil_rc_inputs_raw_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_hil_rc_inputs_raw_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_hil_rc_inputs_raw_t, chan12_raw) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_hil_rc_inputs_raw_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a hil_rc_inputs_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param chan9_raw RC channel 9 value, in microseconds + * @param chan10_raw RC channel 10 value, in microseconds + * @param chan11_raw RC channel 11 value, in microseconds + * @param chan12_raw RC channel 12 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +} + +/** + * @brief Pack a hil_rc_inputs_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param chan9_raw RC channel 9 value, in microseconds + * @param chan10_raw RC channel 10 value, in microseconds + * @param chan11_raw RC channel 11 value, in microseconds + * @param chan12_raw RC channel 12 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +} + +/** + * @brief Encode a hil_rc_inputs_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + +/** + * @brief Encode a hil_rc_inputs_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + +/** + * @brief Send a hil_rc_inputs_raw message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param chan9_raw RC channel 9 value, in microseconds + * @param chan10_raw RC channel 10 value, in microseconds + * @param chan11_raw RC channel 11 value, in microseconds + * @param chan12_raw RC channel 12 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#else + mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_RC_INPUTS_RAW UNPACKING + + +/** + * @brief Get field time_usec from hil_rc_inputs_raw message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field chan1_raw from hil_rc_inputs_raw message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan2_raw from hil_rc_inputs_raw message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan3_raw from hil_rc_inputs_raw message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan4_raw from hil_rc_inputs_raw message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan5_raw from hil_rc_inputs_raw message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field chan6_raw from hil_rc_inputs_raw message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan7_raw from hil_rc_inputs_raw message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan8_raw from hil_rc_inputs_raw message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan9_raw from hil_rc_inputs_raw message + * + * @return RC channel 9 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan10_raw from hil_rc_inputs_raw message + * + * @return RC channel 10 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan11_raw from hil_rc_inputs_raw message + * + * @return RC channel 11 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan12_raw from hil_rc_inputs_raw message + * + * @return RC channel 12 value, in microseconds + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field rssi from hil_rc_inputs_raw message + * + * @return Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Decode a hil_rc_inputs_raw message into a struct + * + * @param msg The message to decode + * @param hil_rc_inputs_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t* msg, mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_rc_inputs_raw->time_usec = mavlink_msg_hil_rc_inputs_raw_get_time_usec(msg); + hil_rc_inputs_raw->chan1_raw = mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(msg); + hil_rc_inputs_raw->chan2_raw = mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(msg); + hil_rc_inputs_raw->chan3_raw = mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(msg); + hil_rc_inputs_raw->chan4_raw = mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(msg); + hil_rc_inputs_raw->chan5_raw = mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(msg); + hil_rc_inputs_raw->chan6_raw = mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(msg); + hil_rc_inputs_raw->chan7_raw = mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(msg); + hil_rc_inputs_raw->chan8_raw = mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(msg); + hil_rc_inputs_raw->chan9_raw = mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(msg); + hil_rc_inputs_raw->chan10_raw = mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(msg); + hil_rc_inputs_raw->chan11_raw = mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(msg); + hil_rc_inputs_raw->chan12_raw = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(msg); + hil_rc_inputs_raw->rssi = mavlink_msg_hil_rc_inputs_raw_get_rssi(msg); +#else + memcpy(hil_rc_inputs_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_sensor.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_sensor.h new file mode 100644 index 0000000..8672f8b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_sensor.h @@ -0,0 +1,545 @@ +// MESSAGE HIL_SENSOR PACKING + +#define MAVLINK_MSG_ID_HIL_SENSOR 107 + +typedef struct __mavlink_hil_sensor_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float xacc; ///< X acceleration (m/s^2) + float yacc; ///< Y acceleration (m/s^2) + float zacc; ///< Z acceleration (m/s^2) + float xgyro; ///< Angular speed around X axis in body frame (rad / sec) + float ygyro; ///< Angular speed around Y axis in body frame (rad / sec) + float zgyro; ///< Angular speed around Z axis in body frame (rad / sec) + float xmag; ///< X Magnetic field (Gauss) + float ymag; ///< Y Magnetic field (Gauss) + float zmag; ///< Z Magnetic field (Gauss) + float abs_pressure; ///< Absolute pressure in millibar + float diff_pressure; ///< Differential pressure (airspeed) in millibar + float pressure_alt; ///< Altitude calculated from pressure + float temperature; ///< Temperature in degrees celsius + uint32_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature +} mavlink_hil_sensor_t; + +#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64 +#define MAVLINK_MSG_ID_107_LEN 64 + +#define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108 +#define MAVLINK_MSG_ID_107_CRC 108 + + + +#define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ + "HIL_SENSOR", \ + 15, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_sensor_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_sensor_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_sensor_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_sensor_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_sensor_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_sensor_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_hil_sensor_t, zmag) }, \ + { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_hil_sensor_t, abs_pressure) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_hil_sensor_t, diff_pressure) }, \ + { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ + { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + } \ +} + + +/** + * @brief Pack a hil_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a hil_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a hil_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + +/** + * @brief Encode a hil_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + +/** + * @brief Send a hil_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_SENSOR UNPACKING + + +/** + * @brief Get field time_usec from hil_sensor message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_hil_sensor_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from hil_sensor message + * + * @return X acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yacc from hil_sensor message + * + * @return Y acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field zacc from hil_sensor message + * + * @return Z acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field xgyro from hil_sensor message + * + * @return Angular speed around X axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field ygyro from hil_sensor message + * + * @return Angular speed around Y axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field zgyro from hil_sensor message + * + * @return Angular speed around Z axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field xmag from hil_sensor message + * + * @return X Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field ymag from hil_sensor message + * + * @return Y Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field zmag from hil_sensor message + * + * @return Z Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field abs_pressure from hil_sensor message + * + * @return Absolute pressure in millibar + */ +static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field diff_pressure from hil_sensor message + * + * @return Differential pressure (airspeed) in millibar + */ +static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field pressure_alt from hil_sensor message + * + * @return Altitude calculated from pressure + */ +static inline float mavlink_msg_hil_sensor_get_pressure_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field temperature from hil_sensor message + * + * @return Temperature in degrees celsius + */ +static inline float mavlink_msg_hil_sensor_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field fields_updated from hil_sensor message + * + * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 60); +} + +/** + * @brief Decode a hil_sensor message into a struct + * + * @param msg The message to decode + * @param hil_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, mavlink_hil_sensor_t* hil_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_sensor->time_usec = mavlink_msg_hil_sensor_get_time_usec(msg); + hil_sensor->xacc = mavlink_msg_hil_sensor_get_xacc(msg); + hil_sensor->yacc = mavlink_msg_hil_sensor_get_yacc(msg); + hil_sensor->zacc = mavlink_msg_hil_sensor_get_zacc(msg); + hil_sensor->xgyro = mavlink_msg_hil_sensor_get_xgyro(msg); + hil_sensor->ygyro = mavlink_msg_hil_sensor_get_ygyro(msg); + hil_sensor->zgyro = mavlink_msg_hil_sensor_get_zgyro(msg); + hil_sensor->xmag = mavlink_msg_hil_sensor_get_xmag(msg); + hil_sensor->ymag = mavlink_msg_hil_sensor_get_ymag(msg); + hil_sensor->zmag = mavlink_msg_hil_sensor_get_zmag(msg); + hil_sensor->abs_pressure = mavlink_msg_hil_sensor_get_abs_pressure(msg); + hil_sensor->diff_pressure = mavlink_msg_hil_sensor_get_diff_pressure(msg); + hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg); + hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg); + hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg); +#else + memcpy(hil_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_state.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_state.h new file mode 100644 index 0000000..923ed60 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_state.h @@ -0,0 +1,569 @@ +// MESSAGE HIL_STATE PACKING + +#define MAVLINK_MSG_ID_HIL_STATE 90 + +typedef struct __mavlink_hil_state_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Body frame roll / phi angular speed (rad/s) + float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) + float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) +} mavlink_hil_state_t; + +#define MAVLINK_MSG_ID_HIL_STATE_LEN 56 +#define MAVLINK_MSG_ID_90_LEN 56 + +#define MAVLINK_MSG_ID_HIL_STATE_CRC 183 +#define MAVLINK_MSG_ID_90_CRC 183 + + + +#define MAVLINK_MESSAGE_INFO_HIL_STATE { \ + "HIL_STATE", \ + 16, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_usec) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \ + } \ +} + + +/** + * @brief Pack a hil_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +} + +/** + * @brief Pack a hil_state message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,int16_t xacc,int16_t yacc,int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +} + +/** + * @brief Encode a hil_state struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** + * @brief Encode a hil_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** + * @brief Send a hil_state message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#else + mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_STATE UNPACKING + + +/** + * @brief Get field time_usec from hil_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll from hil_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch from hil_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw from hil_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rollspeed from hil_state message + * + * @return Body frame roll / phi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from hil_state message + * + * @return Body frame pitch / theta angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from hil_state message + * + * @return Body frame yaw / psi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field lat from hil_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Get field lon from hil_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field alt from hil_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 40); +} + +/** + * @brief Get field vx from hil_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 44); +} + +/** + * @brief Get field vy from hil_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 46); +} + +/** + * @brief Get field vz from hil_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 48); +} + +/** + * @brief Get field xacc from hil_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 50); +} + +/** + * @brief Get field yacc from hil_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 52); +} + +/** + * @brief Get field zacc from hil_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 54); +} + +/** + * @brief Decode a hil_state message into a struct + * + * @param msg The message to decode + * @param hil_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_state->time_usec = mavlink_msg_hil_state_get_time_usec(msg); + hil_state->roll = mavlink_msg_hil_state_get_roll(msg); + hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg); + hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg); + hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg); + hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg); + hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg); + hil_state->lat = mavlink_msg_hil_state_get_lat(msg); + hil_state->lon = mavlink_msg_hil_state_get_lon(msg); + hil_state->alt = mavlink_msg_hil_state_get_alt(msg); + hil_state->vx = mavlink_msg_hil_state_get_vx(msg); + hil_state->vy = mavlink_msg_hil_state_get_vy(msg); + hil_state->vz = mavlink_msg_hil_state_get_vz(msg); + hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg); + hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); + hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); +#else + memcpy(hil_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_hil_state_quaternion.h b/flightcode/mavlink/c_library/common/mavlink_msg_hil_state_quaternion.h new file mode 100644 index 0000000..344a19e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_hil_state_quaternion.h @@ -0,0 +1,561 @@ +// MESSAGE HIL_STATE_QUATERNION PACKING + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION 115 + +typedef struct __mavlink_hil_state_quaternion_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + float rollspeed; ///< Body frame roll / phi angular speed (rad/s) + float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) + float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + uint16_t ind_airspeed; ///< Indicated airspeed, expressed as m/s * 100 + uint16_t true_airspeed; ///< True airspeed, expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) +} mavlink_hil_state_quaternion_t; + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN 64 +#define MAVLINK_MSG_ID_115_LEN 64 + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC 4 +#define MAVLINK_MSG_ID_115_CRC 4 + +#define MAVLINK_MSG_HIL_STATE_QUATERNION_FIELD_ATTITUDE_QUATERNION_LEN 4 + +#define MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION { \ + "HIL_STATE_QUATERNION", \ + 16, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_quaternion_t, time_usec) }, \ + { "attitude_quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_hil_state_quaternion_t, attitude_quaternion) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_quaternion_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_quaternion_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_state_quaternion_t, yawspeed) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_quaternion_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_quaternion_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_hil_state_quaternion_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_quaternion_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_quaternion_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_quaternion_t, vz) }, \ + { "ind_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 54, offsetof(mavlink_hil_state_quaternion_t, ind_airspeed) }, \ + { "true_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_hil_state_quaternion_t, true_airspeed) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_hil_state_quaternion_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_hil_state_quaternion_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_hil_state_quaternion_t, zacc) }, \ + } \ +} + + +/** + * @brief Pack a hil_state_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} + +/** + * @brief Pack a hil_state_quaternion message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,const float *attitude_quaternion,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,uint16_t ind_airspeed,uint16_t true_airspeed,int16_t xacc,int16_t yacc,int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} + +/** + * @brief Encode a hil_state_quaternion struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack(system_id, component_id, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + +/** + * @brief Encode a hil_state_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + +/** + * @brief Send a hil_state_quaternion message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf; + packet->time_usec = time_usec; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ind_airspeed = ind_airspeed; + packet->true_airspeed = true_airspeed; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE HIL_STATE_QUATERNION UNPACKING + + +/** + * @brief Get field time_usec from hil_state_quaternion message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field attitude_quaternion from hil_state_quaternion message + * + * @return Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion) +{ + return _MAV_RETURN_float_array(msg, attitude_quaternion, 4, 8); +} + +/** + * @brief Get field rollspeed from hil_state_quaternion message + * + * @return Body frame roll / phi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field pitchspeed from hil_state_quaternion message + * + * @return Body frame pitch / theta angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field yawspeed from hil_state_quaternion message + * + * @return Body frame yaw / psi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field lat from hil_state_quaternion message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field lon from hil_state_quaternion message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 40); +} + +/** + * @brief Get field alt from hil_state_quaternion message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 44); +} + +/** + * @brief Get field vx from hil_state_quaternion message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 48); +} + +/** + * @brief Get field vy from hil_state_quaternion message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 50); +} + +/** + * @brief Get field vz from hil_state_quaternion message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 52); +} + +/** + * @brief Get field ind_airspeed from hil_state_quaternion message + * + * @return Indicated airspeed, expressed as m/s * 100 + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_ind_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 54); +} + +/** + * @brief Get field true_airspeed from hil_state_quaternion message + * + * @return True airspeed, expressed as m/s * 100 + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_true_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 56); +} + +/** + * @brief Get field xacc from hil_state_quaternion message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 58); +} + +/** + * @brief Get field yacc from hil_state_quaternion message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 60); +} + +/** + * @brief Get field zacc from hil_state_quaternion message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 62); +} + +/** + * @brief Decode a hil_state_quaternion message into a struct + * + * @param msg The message to decode + * @param hil_state_quaternion C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_state_quaternion_decode(const mavlink_message_t* msg, mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_state_quaternion->time_usec = mavlink_msg_hil_state_quaternion_get_time_usec(msg); + mavlink_msg_hil_state_quaternion_get_attitude_quaternion(msg, hil_state_quaternion->attitude_quaternion); + hil_state_quaternion->rollspeed = mavlink_msg_hil_state_quaternion_get_rollspeed(msg); + hil_state_quaternion->pitchspeed = mavlink_msg_hil_state_quaternion_get_pitchspeed(msg); + hil_state_quaternion->yawspeed = mavlink_msg_hil_state_quaternion_get_yawspeed(msg); + hil_state_quaternion->lat = mavlink_msg_hil_state_quaternion_get_lat(msg); + hil_state_quaternion->lon = mavlink_msg_hil_state_quaternion_get_lon(msg); + hil_state_quaternion->alt = mavlink_msg_hil_state_quaternion_get_alt(msg); + hil_state_quaternion->vx = mavlink_msg_hil_state_quaternion_get_vx(msg); + hil_state_quaternion->vy = mavlink_msg_hil_state_quaternion_get_vy(msg); + hil_state_quaternion->vz = mavlink_msg_hil_state_quaternion_get_vz(msg); + hil_state_quaternion->ind_airspeed = mavlink_msg_hil_state_quaternion_get_ind_airspeed(msg); + hil_state_quaternion->true_airspeed = mavlink_msg_hil_state_quaternion_get_true_airspeed(msg); + hil_state_quaternion->xacc = mavlink_msg_hil_state_quaternion_get_xacc(msg); + hil_state_quaternion->yacc = mavlink_msg_hil_state_quaternion_get_yacc(msg); + hil_state_quaternion->zacc = mavlink_msg_hil_state_quaternion_get_zacc(msg); +#else + memcpy(hil_state_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_landing_target.h b/flightcode/mavlink/c_library/common/mavlink_msg_landing_target.h new file mode 100644 index 0000000..ef7bd40 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_landing_target.h @@ -0,0 +1,305 @@ +// MESSAGE LANDING_TARGET PACKING + +#define MAVLINK_MSG_ID_LANDING_TARGET 149 + +typedef struct __mavlink_landing_target_t +{ + float angle_x; ///< X-axis angular offset (in radians) of the target from the center of the image + float angle_y; ///< Y-axis angular offset (in radians) of the target from the center of the image + float distance; ///< Distance to the target from the vehicle in meters + uint8_t target_num; ///< The ID of the target if multiple targets are present + uint8_t frame; ///< MAV_FRAME enum specifying the whether the following feilds are earth-frame, body-frame, etc. +} mavlink_landing_target_t; + +#define MAVLINK_MSG_ID_LANDING_TARGET_LEN 14 +#define MAVLINK_MSG_ID_149_LEN 14 + +#define MAVLINK_MSG_ID_LANDING_TARGET_CRC 255 +#define MAVLINK_MSG_ID_149_CRC 255 + + + +#define MAVLINK_MESSAGE_INFO_LANDING_TARGET { \ + "LANDING_TARGET", \ + 5, \ + { { "angle_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_landing_target_t, angle_x) }, \ + { "angle_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_landing_target_t, angle_y) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_landing_target_t, distance) }, \ + { "target_num", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_landing_target_t, target_num) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_landing_target_t, frame) }, \ + } \ +} + + +/** + * @brief Pack a landing_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_num The ID of the target if multiple targets are present + * @param frame MAV_FRAME enum specifying the whether the following feilds are earth-frame, body-frame, etc. + * @param angle_x X-axis angular offset (in radians) of the target from the center of the image + * @param angle_y Y-axis angular offset (in radians) of the target from the center of the image + * @param distance Distance to the target from the vehicle in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; + _mav_put_float(buf, 0, angle_x); + _mav_put_float(buf, 4, angle_y); + _mav_put_float(buf, 8, distance); + _mav_put_uint8_t(buf, 12, target_num); + _mav_put_uint8_t(buf, 13, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#else + mavlink_landing_target_t packet; + packet.angle_x = angle_x; + packet.angle_y = angle_y; + packet.distance = distance; + packet.target_num = target_num; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LANDING_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +} + +/** + * @brief Pack a landing_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_num The ID of the target if multiple targets are present + * @param frame MAV_FRAME enum specifying the whether the following feilds are earth-frame, body-frame, etc. + * @param angle_x X-axis angular offset (in radians) of the target from the center of the image + * @param angle_y Y-axis angular offset (in radians) of the target from the center of the image + * @param distance Distance to the target from the vehicle in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_num,uint8_t frame,float angle_x,float angle_y,float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; + _mav_put_float(buf, 0, angle_x); + _mav_put_float(buf, 4, angle_y); + _mav_put_float(buf, 8, distance); + _mav_put_uint8_t(buf, 12, target_num); + _mav_put_uint8_t(buf, 13, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#else + mavlink_landing_target_t packet; + packet.angle_x = angle_x; + packet.angle_y = angle_y; + packet.distance = distance; + packet.target_num = target_num; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LANDING_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +} + +/** + * @brief Encode a landing_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param landing_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_landing_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) +{ + return mavlink_msg_landing_target_pack(system_id, component_id, msg, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance); +} + +/** + * @brief Encode a landing_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param landing_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) +{ + return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance); +} + +/** + * @brief Send a landing_target message + * @param chan MAVLink channel to send the message + * + * @param target_num The ID of the target if multiple targets are present + * @param frame MAV_FRAME enum specifying the whether the following feilds are earth-frame, body-frame, etc. + * @param angle_x X-axis angular offset (in radians) of the target from the center of the image + * @param angle_y Y-axis angular offset (in radians) of the target from the center of the image + * @param distance Distance to the target from the vehicle in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; + _mav_put_float(buf, 0, angle_x); + _mav_put_float(buf, 4, angle_y); + _mav_put_float(buf, 8, distance); + _mav_put_uint8_t(buf, 12, target_num); + _mav_put_uint8_t(buf, 13, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +#else + mavlink_landing_target_t packet; + packet.angle_x = angle_x; + packet.angle_y = angle_y; + packet.distance = distance; + packet.target_num = target_num; + packet.frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)&packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)&packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LANDING_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, angle_x); + _mav_put_float(buf, 4, angle_y); + _mav_put_float(buf, 8, distance); + _mav_put_uint8_t(buf, 12, target_num); + _mav_put_uint8_t(buf, 13, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +#else + mavlink_landing_target_t *packet = (mavlink_landing_target_t *)msgbuf; + packet->angle_x = angle_x; + packet->angle_y = angle_y; + packet->distance = distance; + packet->target_num = target_num; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LANDING_TARGET UNPACKING + + +/** + * @brief Get field target_num from landing_target message + * + * @return The ID of the target if multiple targets are present + */ +static inline uint8_t mavlink_msg_landing_target_get_target_num(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field frame from landing_target message + * + * @return MAV_FRAME enum specifying the whether the following feilds are earth-frame, body-frame, etc. + */ +static inline uint8_t mavlink_msg_landing_target_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field angle_x from landing_target message + * + * @return X-axis angular offset (in radians) of the target from the center of the image + */ +static inline float mavlink_msg_landing_target_get_angle_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field angle_y from landing_target message + * + * @return Y-axis angular offset (in radians) of the target from the center of the image + */ +static inline float mavlink_msg_landing_target_get_angle_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field distance from landing_target message + * + * @return Distance to the target from the vehicle in meters + */ +static inline float mavlink_msg_landing_target_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a landing_target message into a struct + * + * @param msg The message to decode + * @param landing_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_landing_target_decode(const mavlink_message_t* msg, mavlink_landing_target_t* landing_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + landing_target->angle_x = mavlink_msg_landing_target_get_angle_x(msg); + landing_target->angle_y = mavlink_msg_landing_target_get_angle_y(msg); + landing_target->distance = mavlink_msg_landing_target_get_distance(msg); + landing_target->target_num = mavlink_msg_landing_target_get_target_num(msg); + landing_target->frame = mavlink_msg_landing_target_get_frame(msg); +#else + memcpy(landing_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned.h b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned.h new file mode 100644 index 0000000..e18a948 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned.h @@ -0,0 +1,353 @@ +// MESSAGE LOCAL_POSITION_NED PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED 32 + +typedef struct __mavlink_local_position_ned_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float vx; ///< X Speed + float vy; ///< Y Speed + float vz; ///< Z Speed +} mavlink_local_position_ned_t; + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN 28 +#define MAVLINK_MSG_ID_32_LEN 28 + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC 185 +#define MAVLINK_MSG_ID_32_CRC 185 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED { \ + "LOCAL_POSITION_NED", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_t, vz) }, \ + } \ +} + + +/** + * @brief Pack a local_position_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#else + mavlink_local_position_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +} + +/** + * @brief Pack a local_position_ned message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float x,float y,float z,float vx,float vy,float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#else + mavlink_local_position_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +} + +/** + * @brief Encode a local_position_ned struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack(system_id, component_id, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + +/** + * @brief Encode a local_position_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + +/** + * @brief Send a local_position_ned message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#else + mavlink_local_position_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#else + mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOCAL_POSITION_NED UNPACKING + + +/** + * @brief Get field time_boot_ms from local_position_ned message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_local_position_ned_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field x from local_position_ned message + * + * @return X Position + */ +static inline float mavlink_msg_local_position_ned_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from local_position_ned message + * + * @return Y Position + */ +static inline float mavlink_msg_local_position_ned_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from local_position_ned message + * + * @return Z Position + */ +static inline float mavlink_msg_local_position_ned_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from local_position_ned message + * + * @return X Speed + */ +static inline float mavlink_msg_local_position_ned_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from local_position_ned message + * + * @return Y Speed + */ +static inline float mavlink_msg_local_position_ned_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from local_position_ned message + * + * @return Z Speed + */ +static inline float mavlink_msg_local_position_ned_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a local_position_ned message into a struct + * + * @param msg The message to decode + * @param local_position_ned C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_position_ned_decode(const mavlink_message_t* msg, mavlink_local_position_ned_t* local_position_ned) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_position_ned->time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(msg); + local_position_ned->x = mavlink_msg_local_position_ned_get_x(msg); + local_position_ned->y = mavlink_msg_local_position_ned_get_y(msg); + local_position_ned->z = mavlink_msg_local_position_ned_get_z(msg); + local_position_ned->vx = mavlink_msg_local_position_ned_get_vx(msg); + local_position_ned->vy = mavlink_msg_local_position_ned_get_vy(msg); + local_position_ned->vz = mavlink_msg_local_position_ned_get_vz(msg); +#else + memcpy(local_position_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_cov.h b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_cov.h new file mode 100644 index 0000000..2793422 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_cov.h @@ -0,0 +1,489 @@ +// MESSAGE LOCAL_POSITION_NED_COV PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV 64 + +typedef struct __mavlink_local_position_ned_cov_t +{ + uint64_t time_utc; ///< Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot). 0 for system without monotonic timestamp + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float vx; ///< X Speed (m/s) + float vy; ///< Y Speed (m/s) + float vz; ///< Z Speed (m/s) + float ax; ///< X Acceleration (m/s^2) + float ay; ///< Y Acceleration (m/s^2) + float az; ///< Z Acceleration (m/s^2) + float covariance[45]; ///< Covariance matrix upper right triangular (first nine entries are the first ROW, next eight entries are the second row, etc.) + uint8_t estimator_type; ///< Class id of the estimator this estimate originated from. +} mavlink_local_position_ned_cov_t; + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN 229 +#define MAVLINK_MSG_ID_64_LEN 229 + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC 59 +#define MAVLINK_MSG_ID_64_CRC 59 + +#define MAVLINK_MSG_LOCAL_POSITION_NED_COV_FIELD_COVARIANCE_LEN 45 + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV { \ + "LOCAL_POSITION_NED_COV", \ + 13, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_ned_cov_t, time_utc) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_local_position_ned_cov_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_cov_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_cov_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_cov_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_cov_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_ned_cov_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_position_ned_cov_t, vz) }, \ + { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_position_ned_cov_t, ax) }, \ + { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_local_position_ned_cov_t, ay) }, \ + { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_local_position_ned_cov_t, az) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 45, 48, offsetof(mavlink_local_position_ned_cov_t, covariance) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_local_position_ned_cov_t, estimator_type) }, \ + } \ +} + + +/** + * @brief Pack a local_position_ned_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot). 0 for system without monotonic timestamp + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed (m/s) + * @param vy Y Speed (m/s) + * @param vz Z Speed (m/s) + * @param ax X Acceleration (m/s^2) + * @param ay Y Acceleration (m/s^2) + * @param az Z Acceleration (m/s^2) + * @param covariance Covariance matrix upper right triangular (first nine entries are the first ROW, next eight entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_float(buf, 36, ax); + _mav_put_float(buf, 40, ay); + _mav_put_float(buf, 44, az); + _mav_put_uint8_t(buf, 228, estimator_type); + _mav_put_float_array(buf, 48, covariance, 45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} + +/** + * @brief Pack a local_position_ned_cov message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot). 0 for system without monotonic timestamp + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed (m/s) + * @param vy Y Speed (m/s) + * @param vz Z Speed (m/s) + * @param ax X Acceleration (m/s^2) + * @param ay Y Acceleration (m/s^2) + * @param az Z Acceleration (m/s^2) + * @param covariance Covariance matrix upper right triangular (first nine entries are the first ROW, next eight entries are the second row, etc.) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t estimator_type,float x,float y,float z,float vx,float vy,float vz,float ax,float ay,float az,const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_float(buf, 36, ax); + _mav_put_float(buf, 40, ay); + _mav_put_float(buf, 44, az); + _mav_put_uint8_t(buf, 228, estimator_type); + _mav_put_float_array(buf, 48, covariance, 45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} + +/** + * @brief Encode a local_position_ned_cov struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ + return mavlink_msg_local_position_ned_cov_pack(system_id, component_id, msg, local_position_ned_cov->time_boot_ms, local_position_ned_cov->time_utc, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance); +} + +/** + * @brief Encode a local_position_ned_cov struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ + return mavlink_msg_local_position_ned_cov_pack_chan(system_id, component_id, chan, msg, local_position_ned_cov->time_boot_ms, local_position_ned_cov->time_utc, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance); +} + +/** + * @brief Send a local_position_ned_cov message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot). 0 for system without monotonic timestamp + * @param time_utc Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed (m/s) + * @param vy Y Speed (m/s) + * @param vz Z Speed (m/s) + * @param ax X Acceleration (m/s^2) + * @param ay Y Acceleration (m/s^2) + * @param az Z Acceleration (m/s^2) + * @param covariance Covariance matrix upper right triangular (first nine entries are the first ROW, next eight entries are the second row, etc.) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_local_position_ned_cov_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_float(buf, 36, ax); + _mav_put_float(buf, 40, ay); + _mav_put_float(buf, 44, az); + _mav_put_uint8_t(buf, 228, estimator_type); + _mav_put_float_array(buf, 48, covariance, 45); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#else + mavlink_local_position_ned_cov_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_float(buf, 36, ax); + _mav_put_float(buf, 40, ay); + _mav_put_float(buf, 44, az); + _mav_put_uint8_t(buf, 228, estimator_type); + _mav_put_float_array(buf, 48, covariance, 45); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#else + mavlink_local_position_ned_cov_t *packet = (mavlink_local_position_ned_cov_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ax = ax; + packet->ay = ay; + packet->az = az; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*45); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOCAL_POSITION_NED_COV UNPACKING + + +/** + * @brief Get field time_boot_ms from local_position_ned_cov message + * + * @return Timestamp (milliseconds since system boot). 0 for system without monotonic timestamp + */ +static inline uint32_t mavlink_msg_local_position_ned_cov_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from local_position_ned_cov message + * + * @return Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. + */ +static inline uint64_t mavlink_msg_local_position_ned_cov_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field estimator_type from local_position_ned_cov message + * + * @return Class id of the estimator this estimate originated from. + */ +static inline uint8_t mavlink_msg_local_position_ned_cov_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 228); +} + +/** + * @brief Get field x from local_position_ned_cov message + * + * @return X Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field y from local_position_ned_cov message + * + * @return Y Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field z from local_position_ned_cov message + * + * @return Z Position + */ +static inline float mavlink_msg_local_position_ned_cov_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vx from local_position_ned_cov message + * + * @return X Speed (m/s) + */ +static inline float mavlink_msg_local_position_ned_cov_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vy from local_position_ned_cov message + * + * @return Y Speed (m/s) + */ +static inline float mavlink_msg_local_position_ned_cov_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vz from local_position_ned_cov message + * + * @return Z Speed (m/s) + */ +static inline float mavlink_msg_local_position_ned_cov_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field ax from local_position_ned_cov message + * + * @return X Acceleration (m/s^2) + */ +static inline float mavlink_msg_local_position_ned_cov_get_ax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field ay from local_position_ned_cov message + * + * @return Y Acceleration (m/s^2) + */ +static inline float mavlink_msg_local_position_ned_cov_get_ay(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field az from local_position_ned_cov message + * + * @return Z Acceleration (m/s^2) + */ +static inline float mavlink_msg_local_position_ned_cov_get_az(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field covariance from local_position_ned_cov message + * + * @return Covariance matrix upper right triangular (first nine entries are the first ROW, next eight entries are the second row, etc.) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 45, 48); +} + +/** + * @brief Decode a local_position_ned_cov message into a struct + * + * @param msg The message to decode + * @param local_position_ned_cov C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_position_ned_cov_decode(const mavlink_message_t* msg, mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_position_ned_cov->time_utc = mavlink_msg_local_position_ned_cov_get_time_utc(msg); + local_position_ned_cov->time_boot_ms = mavlink_msg_local_position_ned_cov_get_time_boot_ms(msg); + local_position_ned_cov->x = mavlink_msg_local_position_ned_cov_get_x(msg); + local_position_ned_cov->y = mavlink_msg_local_position_ned_cov_get_y(msg); + local_position_ned_cov->z = mavlink_msg_local_position_ned_cov_get_z(msg); + local_position_ned_cov->vx = mavlink_msg_local_position_ned_cov_get_vx(msg); + local_position_ned_cov->vy = mavlink_msg_local_position_ned_cov_get_vy(msg); + local_position_ned_cov->vz = mavlink_msg_local_position_ned_cov_get_vz(msg); + local_position_ned_cov->ax = mavlink_msg_local_position_ned_cov_get_ax(msg); + local_position_ned_cov->ay = mavlink_msg_local_position_ned_cov_get_ay(msg); + local_position_ned_cov->az = mavlink_msg_local_position_ned_cov_get_az(msg); + mavlink_msg_local_position_ned_cov_get_covariance(msg, local_position_ned_cov->covariance); + local_position_ned_cov->estimator_type = mavlink_msg_local_position_ned_cov_get_estimator_type(msg); +#else + memcpy(local_position_ned_cov, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_system_global_offset.h b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_system_global_offset.h new file mode 100644 index 0000000..af7d195 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -0,0 +1,353 @@ +// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET 89 + +typedef struct __mavlink_local_position_ned_system_global_offset_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float roll; ///< Roll + float pitch; ///< Pitch + float yaw; ///< Yaw +} mavlink_local_position_ned_system_global_offset_t; + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN 28 +#define MAVLINK_MSG_ID_89_LEN 28 + +#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC 231 +#define MAVLINK_MSG_ID_89_CRC 231 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET { \ + "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_system_global_offset_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_system_global_offset_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_system_global_offset_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_system_global_offset_t, z) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_system_global_offset_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_system_global_offset_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_system_global_offset_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a local_position_ned_system_global_offset message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param roll Roll + * @param pitch Pitch + * @param yaw Yaw + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#else + mavlink_local_position_ned_system_global_offset_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +} + +/** + * @brief Pack a local_position_ned_system_global_offset message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param roll Roll + * @param pitch Pitch + * @param yaw Yaw + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float x,float y,float z,float roll,float pitch,float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#else + mavlink_local_position_ned_system_global_offset_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +} + +/** + * @brief Encode a local_position_ned_system_global_offset struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + +/** + * @brief Encode a local_position_ned_system_global_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + +/** + * @brief Send a local_position_ned_system_global_offset message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param roll Roll + * @param pitch Pitch + * @param yaw Yaw + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#else + mavlink_local_position_ned_system_global_offset_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#else + mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING + + +/** + * @brief Get field time_boot_ms from local_position_ned_system_global_offset message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field x from local_position_ned_system_global_offset message + * + * @return X Position + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from local_position_ned_system_global_offset message + * + * @return Y Position + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from local_position_ned_system_global_offset message + * + * @return Z Position + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field roll from local_position_ned_system_global_offset message + * + * @return Roll + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field pitch from local_position_ned_system_global_offset message + * + * @return Pitch + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field yaw from local_position_ned_system_global_offset message + * + * @return Yaw + */ +static inline float mavlink_msg_local_position_ned_system_global_offset_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a local_position_ned_system_global_offset message into a struct + * + * @param msg The message to decode + * @param local_position_ned_system_global_offset C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_position_ned_system_global_offset_decode(const mavlink_message_t* msg, mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_position_ned_system_global_offset->time_boot_ms = mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(msg); + local_position_ned_system_global_offset->x = mavlink_msg_local_position_ned_system_global_offset_get_x(msg); + local_position_ned_system_global_offset->y = mavlink_msg_local_position_ned_system_global_offset_get_y(msg); + local_position_ned_system_global_offset->z = mavlink_msg_local_position_ned_system_global_offset_get_z(msg); + local_position_ned_system_global_offset->roll = mavlink_msg_local_position_ned_system_global_offset_get_roll(msg); + local_position_ned_system_global_offset->pitch = mavlink_msg_local_position_ned_system_global_offset_get_pitch(msg); + local_position_ned_system_global_offset->yaw = mavlink_msg_local_position_ned_system_global_offset_get_yaw(msg); +#else + memcpy(local_position_ned_system_global_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_data.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_data.h new file mode 100644 index 0000000..48641f3 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_data.h @@ -0,0 +1,273 @@ +// MESSAGE LOG_DATA PACKING + +#define MAVLINK_MSG_ID_LOG_DATA 120 + +typedef struct __mavlink_log_data_t +{ + uint32_t ofs; ///< Offset into the log + uint16_t id; ///< Log id (from LOG_ENTRY reply) + uint8_t count; ///< Number of bytes (zero for end of log) + uint8_t data[90]; ///< log data +} mavlink_log_data_t; + +#define MAVLINK_MSG_ID_LOG_DATA_LEN 97 +#define MAVLINK_MSG_ID_120_LEN 97 + +#define MAVLINK_MSG_ID_LOG_DATA_CRC 134 +#define MAVLINK_MSG_ID_120_CRC 134 + +#define MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN 90 + +#define MAVLINK_MESSAGE_INFO_LOG_DATA { \ + "LOG_DATA", \ + 4, \ + { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_data_t, ofs) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_log_data_t, id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_log_data_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 90, 7, offsetof(mavlink_log_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a log_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} + +/** + * @brief Pack a log_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t id,uint32_t ofs,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} + +/** + * @brief Encode a log_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_data_t* log_data) +{ + return mavlink_msg_log_data_pack(system_id, component_id, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); +} + +/** + * @brief Encode a log_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_data_t* log_data) +{ + return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); +} + +/** + * @brief Send a log_data message + * @param chan MAVLink channel to send the message + * + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes (zero for end of log) + * @param data log data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf; + packet->ofs = ofs; + packet->id = id; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_DATA UNPACKING + + +/** + * @brief Get field id from log_data message + * + * @return Log id (from LOG_ENTRY reply) + */ +static inline uint16_t mavlink_msg_log_data_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field ofs from log_data message + * + * @return Offset into the log + */ +static inline uint32_t mavlink_msg_log_data_get_ofs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from log_data message + * + * @return Number of bytes (zero for end of log) + */ +static inline uint8_t mavlink_msg_log_data_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field data from log_data message + * + * @return log data + */ +static inline uint16_t mavlink_msg_log_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 90, 7); +} + +/** + * @brief Decode a log_data message into a struct + * + * @param msg The message to decode + * @param log_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_data_decode(const mavlink_message_t* msg, mavlink_log_data_t* log_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_data->ofs = mavlink_msg_log_data_get_ofs(msg); + log_data->id = mavlink_msg_log_data_get_id(msg); + log_data->count = mavlink_msg_log_data_get_count(msg); + mavlink_msg_log_data_get_data(msg, log_data->data); +#else + memcpy(log_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_entry.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_entry.h new file mode 100644 index 0000000..8ecaec3 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_entry.h @@ -0,0 +1,305 @@ +// MESSAGE LOG_ENTRY PACKING + +#define MAVLINK_MSG_ID_LOG_ENTRY 118 + +typedef struct __mavlink_log_entry_t +{ + uint32_t time_utc; ///< UTC timestamp of log in seconds since 1970, or 0 if not available + uint32_t size; ///< Size of the log (may be approximate) in bytes + uint16_t id; ///< Log id + uint16_t num_logs; ///< Total number of logs + uint16_t last_log_num; ///< High log number +} mavlink_log_entry_t; + +#define MAVLINK_MSG_ID_LOG_ENTRY_LEN 14 +#define MAVLINK_MSG_ID_118_LEN 14 + +#define MAVLINK_MSG_ID_LOG_ENTRY_CRC 56 +#define MAVLINK_MSG_ID_118_CRC 56 + + + +#define MAVLINK_MESSAGE_INFO_LOG_ENTRY { \ + "LOG_ENTRY", \ + 5, \ + { { "time_utc", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_entry_t, time_utc) }, \ + { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_entry_t, size) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_entry_t, id) }, \ + { "num_logs", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_log_entry_t, num_logs) }, \ + { "last_log_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_log_entry_t, last_log_num) }, \ + } \ +} + + +/** + * @brief Pack a log_entry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} + +/** + * @brief Pack a log_entry message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_entry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t id,uint16_t num_logs,uint16_t last_log_num,uint32_t time_utc,uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} + +/** + * @brief Encode a log_entry struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_entry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_entry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) +{ + return mavlink_msg_log_entry_pack(system_id, component_id, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); +} + +/** + * @brief Encode a log_entry struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_entry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) +{ + return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); +} + +/** + * @brief Send a log_entry message + * @param chan MAVLink channel to send the message + * + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available + * @param size Size of the log (may be approximate) in bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf; + packet->time_utc = time_utc; + packet->size = size; + packet->id = id; + packet->num_logs = num_logs; + packet->last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_ENTRY UNPACKING + + +/** + * @brief Get field id from log_entry message + * + * @return Log id + */ +static inline uint16_t mavlink_msg_log_entry_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field num_logs from log_entry message + * + * @return Total number of logs + */ +static inline uint16_t mavlink_msg_log_entry_get_num_logs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field last_log_num from log_entry message + * + * @return High log number + */ +static inline uint16_t mavlink_msg_log_entry_get_last_log_num(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field time_utc from log_entry message + * + * @return UTC timestamp of log in seconds since 1970, or 0 if not available + */ +static inline uint32_t mavlink_msg_log_entry_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field size from log_entry message + * + * @return Size of the log (may be approximate) in bytes + */ +static inline uint32_t mavlink_msg_log_entry_get_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a log_entry message into a struct + * + * @param msg The message to decode + * @param log_entry C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_entry_decode(const mavlink_message_t* msg, mavlink_log_entry_t* log_entry) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_entry->time_utc = mavlink_msg_log_entry_get_time_utc(msg); + log_entry->size = mavlink_msg_log_entry_get_size(msg); + log_entry->id = mavlink_msg_log_entry_get_id(msg); + log_entry->num_logs = mavlink_msg_log_entry_get_num_logs(msg); + log_entry->last_log_num = mavlink_msg_log_entry_get_last_log_num(msg); +#else + memcpy(log_entry, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_erase.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_erase.h new file mode 100644 index 0000000..957c4d4 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_erase.h @@ -0,0 +1,233 @@ +// MESSAGE LOG_ERASE PACKING + +#define MAVLINK_MSG_ID_LOG_ERASE 121 + +typedef struct __mavlink_log_erase_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_erase_t; + +#define MAVLINK_MSG_ID_LOG_ERASE_LEN 2 +#define MAVLINK_MSG_ID_121_LEN 2 + +#define MAVLINK_MSG_ID_LOG_ERASE_CRC 237 +#define MAVLINK_MSG_ID_121_CRC 237 + + + +#define MAVLINK_MESSAGE_INFO_LOG_ERASE { \ + "LOG_ERASE", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_erase message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} + +/** + * @brief Pack a log_erase message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} + +/** + * @brief Encode a log_erase struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_erase C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) +{ + return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component); +} + +/** + * @brief Encode a log_erase struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_erase C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) +{ + return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component); +} + +/** + * @brief Send a log_erase message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_ERASE UNPACKING + + +/** + * @brief Get field target_system from log_erase message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from log_erase message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a log_erase message into a struct + * + * @param msg The message to decode + * @param log_erase C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg); + log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg); +#else + memcpy(log_erase, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_request_data.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_data.h new file mode 100644 index 0000000..ef5cbb6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_data.h @@ -0,0 +1,305 @@ +// MESSAGE LOG_REQUEST_DATA PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA 119 + +typedef struct __mavlink_log_request_data_t +{ + uint32_t ofs; ///< Offset into the log + uint32_t count; ///< Number of bytes + uint16_t id; ///< Log id (from LOG_ENTRY reply) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_data_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN 12 +#define MAVLINK_MSG_ID_119_LEN 12 + +#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC 116 +#define MAVLINK_MSG_ID_119_CRC 116 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA { \ + "LOG_REQUEST_DATA", \ + 5, \ + { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_request_data_t, ofs) }, \ + { "count", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_request_data_t, count) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_request_data_t, id) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_log_request_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_log_request_data_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} + +/** + * @brief Pack a log_request_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t id,uint32_t ofs,uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} + +/** + * @brief Encode a log_request_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) +{ + return mavlink_msg_log_request_data_pack(system_id, component_id, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); +} + +/** + * @brief Encode a log_request_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) +{ + return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); +} + +/** + * @brief Send a log_request_data message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count Number of bytes + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf; + packet->ofs = ofs; + packet->count = count; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_REQUEST_DATA UNPACKING + + +/** + * @brief Get field target_system from log_request_data message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field target_component from log_request_data message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field id from log_request_data message + * + * @return Log id (from LOG_ENTRY reply) + */ +static inline uint16_t mavlink_msg_log_request_data_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field ofs from log_request_data message + * + * @return Offset into the log + */ +static inline uint32_t mavlink_msg_log_request_data_get_ofs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from log_request_data message + * + * @return Number of bytes + */ +static inline uint32_t mavlink_msg_log_request_data_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a log_request_data message into a struct + * + * @param msg The message to decode + * @param log_request_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_data_decode(const mavlink_message_t* msg, mavlink_log_request_data_t* log_request_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_data->ofs = mavlink_msg_log_request_data_get_ofs(msg); + log_request_data->count = mavlink_msg_log_request_data_get_count(msg); + log_request_data->id = mavlink_msg_log_request_data_get_id(msg); + log_request_data->target_system = mavlink_msg_log_request_data_get_target_system(msg); + log_request_data->target_component = mavlink_msg_log_request_data_get_target_component(msg); +#else + memcpy(log_request_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_request_end.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_end.h new file mode 100644 index 0000000..23fcca2 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_end.h @@ -0,0 +1,233 @@ +// MESSAGE LOG_REQUEST_END PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_END 122 + +typedef struct __mavlink_log_request_end_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_end_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_END_LEN 2 +#define MAVLINK_MSG_ID_122_LEN 2 + +#define MAVLINK_MSG_ID_LOG_REQUEST_END_CRC 203 +#define MAVLINK_MSG_ID_122_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_END { \ + "LOG_REQUEST_END", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_request_end_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_request_end_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_end message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} + +/** + * @brief Pack a log_request_end message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_end_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} + +/** + * @brief Encode a log_request_end struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_request_end C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_end_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) +{ + return mavlink_msg_log_request_end_pack(system_id, component_id, msg, log_request_end->target_system, log_request_end->target_component); +} + +/** + * @brief Encode a log_request_end struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_end C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) +{ + return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component); +} + +/** + * @brief Send a log_request_end message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_end_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t *packet = (mavlink_log_request_end_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_REQUEST_END UNPACKING + + +/** + * @brief Get field target_system from log_request_end message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_end_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from log_request_end message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_end_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a log_request_end message into a struct + * + * @param msg The message to decode + * @param log_request_end C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_end_decode(const mavlink_message_t* msg, mavlink_log_request_end_t* log_request_end) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_end->target_system = mavlink_msg_log_request_end_get_target_system(msg); + log_request_end->target_component = mavlink_msg_log_request_end_get_target_component(msg); +#else + memcpy(log_request_end, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_log_request_list.h b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_list.h new file mode 100644 index 0000000..e511b53 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_log_request_list.h @@ -0,0 +1,281 @@ +// MESSAGE LOG_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST 117 + +typedef struct __mavlink_log_request_list_t +{ + uint16_t start; ///< First log id (0 for first available) + uint16_t end; ///< Last log id (0xffff for last available) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_log_request_list_t; + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN 6 +#define MAVLINK_MSG_ID_117_LEN 6 + +#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC 128 +#define MAVLINK_MSG_ID_117_CRC 128 + + + +#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST { \ + "LOG_REQUEST_LIST", \ + 4, \ + { { "start", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_log_request_list_t, start) }, \ + { "end", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_log_request_list_t, end) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_log_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_log_request_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a log_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Pack a log_request_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t start,uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Encode a log_request_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param log_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) +{ + return mavlink_msg_log_request_list_pack(system_id, component_id, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); +} + +/** + * @brief Encode a log_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param log_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) +{ + return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); +} + +/** + * @brief Send a log_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf; + packet->start = start; + packet->end = end; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOG_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from log_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_log_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from log_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_log_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start from log_request_list message + * + * @return First log id (0 for first available) + */ +static inline uint16_t mavlink_msg_log_request_list_get_start(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field end from log_request_list message + * + * @return Last log id (0xffff for last available) + */ +static inline uint16_t mavlink_msg_log_request_list_get_end(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a log_request_list message into a struct + * + * @param msg The message to decode + * @param log_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_log_request_list_decode(const mavlink_message_t* msg, mavlink_log_request_list_t* log_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + log_request_list->start = mavlink_msg_log_request_list_get_start(msg); + log_request_list->end = mavlink_msg_log_request_list_get_end(msg); + log_request_list->target_system = mavlink_msg_log_request_list_get_target_system(msg); + log_request_list->target_component = mavlink_msg_log_request_list_get_target_component(msg); +#else + memcpy(log_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_manual_control.h b/flightcode/mavlink/c_library/common/mavlink_msg_manual_control.h new file mode 100644 index 0000000..e93b759 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_manual_control.h @@ -0,0 +1,329 @@ +// MESSAGE MANUAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_MANUAL_CONTROL 69 + +typedef struct __mavlink_manual_control_t +{ + int16_t x; ///< X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + int16_t y; ///< Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + int16_t z; ///< Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. + int16_t r; ///< R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + uint16_t buttons; ///< A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + uint8_t target; ///< The system to be controlled. +} mavlink_manual_control_t; + +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 11 +#define MAVLINK_MSG_ID_69_LEN 11 + +#define MAVLINK_MSG_ID_MANUAL_CONTROL_CRC 243 +#define MAVLINK_MSG_ID_69_CRC 243 + + + +#define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ + "MANUAL_CONTROL", \ + 6, \ + { { "x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_manual_control_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_manual_control_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_manual_control_t, z) }, \ + { "r", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_manual_control_t, r) }, \ + { "buttons", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_manual_control_t, buttons) }, \ + { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_manual_control_t, target) }, \ + } \ +} + + +/** + * @brief Pack a manual_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target The system to be controlled. + * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. + * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#else + mavlink_manual_control_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.r = r; + packet.buttons = buttons; + packet.target = target; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a manual_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target The system to be controlled. + * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. + * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,int16_t x,int16_t y,int16_t z,int16_t r,uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#else + mavlink_manual_control_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.r = r; + packet.buttons = buttons; + packet.target = target; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a manual_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); +} + +/** + * @brief Encode a manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); +} + +/** + * @brief Send a manual_control message + * @param chan MAVLink channel to send the message + * + * @param target The system to be controlled. + * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. + * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#else + mavlink_manual_control_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.r = r; + packet.buttons = buttons; + packet.target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#else + mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->r = r; + packet->buttons = buttons; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MANUAL_CONTROL UNPACKING + + +/** + * @brief Get field target from manual_control message + * + * @return The system to be controlled. + */ +static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field x from manual_control message + * + * @return X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + */ +static inline int16_t mavlink_msg_manual_control_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field y from manual_control message + * + * @return Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + */ +static inline int16_t mavlink_msg_manual_control_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Get field z from manual_control message + * + * @return Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. + */ +static inline int16_t mavlink_msg_manual_control_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field r from manual_control message + * + * @return R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + */ +static inline int16_t mavlink_msg_manual_control_get_r(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field buttons from manual_control message + * + * @return A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + */ +static inline uint16_t mavlink_msg_manual_control_get_buttons(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Decode a manual_control message into a struct + * + * @param msg The message to decode + * @param manual_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + manual_control->x = mavlink_msg_manual_control_get_x(msg); + manual_control->y = mavlink_msg_manual_control_get_y(msg); + manual_control->z = mavlink_msg_manual_control_get_z(msg); + manual_control->r = mavlink_msg_manual_control_get_r(msg); + manual_control->buttons = mavlink_msg_manual_control_get_buttons(msg); + manual_control->target = mavlink_msg_manual_control_get_target(msg); +#else + memcpy(manual_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_manual_setpoint.h b/flightcode/mavlink/c_library/common/mavlink_msg_manual_setpoint.h new file mode 100644 index 0000000..b276267 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_manual_setpoint.h @@ -0,0 +1,353 @@ +// MESSAGE MANUAL_SETPOINT PACKING + +#define MAVLINK_MSG_ID_MANUAL_SETPOINT 81 + +typedef struct __mavlink_manual_setpoint_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float roll; ///< Desired roll rate in radians per second + float pitch; ///< Desired pitch rate in radians per second + float yaw; ///< Desired yaw rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 + uint8_t mode_switch; ///< Flight mode switch position, 0.. 255 + uint8_t manual_override_switch; ///< Override mode switch position, 0.. 255 +} mavlink_manual_setpoint_t; + +#define MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN 22 +#define MAVLINK_MSG_ID_81_LEN 22 + +#define MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC 106 +#define MAVLINK_MSG_ID_81_CRC 106 + + + +#define MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT { \ + "MANUAL_SETPOINT", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_manual_setpoint_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_manual_setpoint_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_manual_setpoint_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_manual_setpoint_t, yaw) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_manual_setpoint_t, thrust) }, \ + { "mode_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_setpoint_t, mode_switch) }, \ + { "manual_override_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_manual_setpoint_t, manual_override_switch) }, \ + } \ +} + + +/** + * @brief Pack a manual_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param roll Desired roll rate in radians per second + * @param pitch Desired pitch rate in radians per second + * @param yaw Desired yaw rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 + * @param mode_switch Flight mode switch position, 0.. 255 + * @param manual_override_switch Override mode switch position, 0.. 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#else + mavlink_manual_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.mode_switch = mode_switch; + packet.manual_override_switch = manual_override_switch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +} + +/** + * @brief Pack a manual_setpoint message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param roll Desired roll rate in radians per second + * @param pitch Desired pitch rate in radians per second + * @param yaw Desired yaw rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 + * @param mode_switch Flight mode switch position, 0.. 255 + * @param manual_override_switch Override mode switch position, 0.. 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust,uint8_t mode_switch,uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#else + mavlink_manual_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.mode_switch = mode_switch; + packet.manual_override_switch = manual_override_switch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +} + +/** + * @brief Encode a manual_setpoint struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack(system_id, component_id, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + +/** + * @brief Encode a manual_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + +/** + * @brief Send a manual_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param roll Desired roll rate in radians per second + * @param pitch Desired pitch rate in radians per second + * @param yaw Desired yaw rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 + * @param mode_switch Flight mode switch position, 0.. 255 + * @param manual_override_switch Override mode switch position, 0.. 255 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#else + mavlink_manual_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.mode_switch = mode_switch; + packet.manual_override_switch = manual_override_switch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#else + mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->mode_switch = mode_switch; + packet->manual_override_switch = manual_override_switch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MANUAL_SETPOINT UNPACKING + + +/** + * @brief Get field time_boot_ms from manual_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_manual_setpoint_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field roll from manual_setpoint message + * + * @return Desired roll rate in radians per second + */ +static inline float mavlink_msg_manual_setpoint_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field pitch from manual_setpoint message + * + * @return Desired pitch rate in radians per second + */ +static inline float mavlink_msg_manual_setpoint_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from manual_setpoint message + * + * @return Desired yaw rate in radians per second + */ +static inline float mavlink_msg_manual_setpoint_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field thrust from manual_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_manual_setpoint_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field mode_switch from manual_setpoint message + * + * @return Flight mode switch position, 0.. 255 + */ +static inline uint8_t mavlink_msg_manual_setpoint_get_mode_switch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field manual_override_switch from manual_setpoint message + * + * @return Override mode switch position, 0.. 255 + */ +static inline uint8_t mavlink_msg_manual_setpoint_get_manual_override_switch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a manual_setpoint message into a struct + * + * @param msg The message to decode + * @param manual_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_manual_setpoint_decode(const mavlink_message_t* msg, mavlink_manual_setpoint_t* manual_setpoint) +{ +#if MAVLINK_NEED_BYTE_SWAP + manual_setpoint->time_boot_ms = mavlink_msg_manual_setpoint_get_time_boot_ms(msg); + manual_setpoint->roll = mavlink_msg_manual_setpoint_get_roll(msg); + manual_setpoint->pitch = mavlink_msg_manual_setpoint_get_pitch(msg); + manual_setpoint->yaw = mavlink_msg_manual_setpoint_get_yaw(msg); + manual_setpoint->thrust = mavlink_msg_manual_setpoint_get_thrust(msg); + manual_setpoint->mode_switch = mavlink_msg_manual_setpoint_get_mode_switch(msg); + manual_setpoint->manual_override_switch = mavlink_msg_manual_setpoint_get_manual_override_switch(msg); +#else + memcpy(manual_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_memory_vect.h b/flightcode/mavlink/c_library/common/mavlink_msg_memory_vect.h new file mode 100644 index 0000000..2eb60cc --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_memory_vect.h @@ -0,0 +1,273 @@ +// MESSAGE MEMORY_VECT PACKING + +#define MAVLINK_MSG_ID_MEMORY_VECT 249 + +typedef struct __mavlink_memory_vect_t +{ + uint16_t address; ///< Starting address of the debug variables + uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + uint8_t type; ///< Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + int8_t value[32]; ///< Memory contents at specified address +} mavlink_memory_vect_t; + +#define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36 +#define MAVLINK_MSG_ID_249_LEN 36 + +#define MAVLINK_MSG_ID_MEMORY_VECT_CRC 204 +#define MAVLINK_MSG_ID_249_CRC 204 + +#define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 + +#define MAVLINK_MESSAGE_INFO_MEMORY_VECT { \ + "MEMORY_VECT", \ + 4, \ + { { "address", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_memory_vect_t, address) }, \ + { "ver", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_memory_vect_t, ver) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_memory_vect_t, type) }, \ + { "value", NULL, MAVLINK_TYPE_INT8_T, 32, 4, offsetof(mavlink_memory_vect_t, value) }, \ + } \ +} + + +/** + * @brief Pack a memory_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +} + +/** + * @brief Pack a memory_vect message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t address,uint8_t ver,uint8_t type,const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +} + +/** + * @brief Encode a memory_vect struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** + * @brief Encode a memory_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** + * @brief Send a memory_vect message + * @param chan MAVLink channel to send the message + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#else + mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf; + packet->address = address; + packet->ver = ver; + packet->type = type; + mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MEMORY_VECT UNPACKING + + +/** + * @brief Get field address from memory_vect message + * + * @return Starting address of the debug variables + */ +static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field ver from memory_vect message + * + * @return Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + */ +static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field type from memory_vect message + * + * @return Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + */ +static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field value from memory_vect message + * + * @return Memory contents at specified address + */ +static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t *value) +{ + return _MAV_RETURN_int8_t_array(msg, value, 32, 4); +} + +/** + * @brief Decode a memory_vect message into a struct + * + * @param msg The message to decode + * @param memory_vect C-struct to decode the message contents into + */ +static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, mavlink_memory_vect_t* memory_vect) +{ +#if MAVLINK_NEED_BYTE_SWAP + memory_vect->address = mavlink_msg_memory_vect_get_address(msg); + memory_vect->ver = mavlink_msg_memory_vect_get_ver(msg); + memory_vect->type = mavlink_msg_memory_vect_get_type(msg); + mavlink_msg_memory_vect_get_value(msg, memory_vect->value); +#else + memcpy(memory_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_ack.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_ack.h new file mode 100644 index 0000000..43afd00 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_ack.h @@ -0,0 +1,257 @@ +// MESSAGE MISSION_ACK PACKING + +#define MAVLINK_MSG_ID_MISSION_ACK 47 + +typedef struct __mavlink_mission_ack_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t type; ///< See MAV_MISSION_RESULT enum +} mavlink_mission_ack_t; + +#define MAVLINK_MSG_ID_MISSION_ACK_LEN 3 +#define MAVLINK_MSG_ID_47_LEN 3 + +#define MAVLINK_MSG_ID_MISSION_ACK_CRC 153 +#define MAVLINK_MSG_ID_47_CRC 153 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ + "MISSION_ACK", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ + } \ +} + + +/** + * @brief Pack a mission_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param type See MAV_MISSION_RESULT enum + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#else + mavlink_mission_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +} + +/** + * @brief Pack a mission_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param type See MAV_MISSION_RESULT enum + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#else + mavlink_mission_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +} + +/** + * @brief Encode a mission_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); +} + +/** + * @brief Encode a mission_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); +} + +/** + * @brief Send a mission_ack message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param type See MAV_MISSION_RESULT enum + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#else + mavlink_mission_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#else + mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type = type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_ACK UNPACKING + + +/** + * @brief Get field target_system from mission_ack message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mission_ack message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field type from mission_ack message + * + * @return See MAV_MISSION_RESULT enum + */ +static inline uint8_t mavlink_msg_mission_ack_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a mission_ack message into a struct + * + * @param msg The message to decode + * @param mission_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mavlink_mission_ack_t* mission_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_ack->target_system = mavlink_msg_mission_ack_get_target_system(msg); + mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg); + mission_ack->type = mavlink_msg_mission_ack_get_type(msg); +#else + memcpy(mission_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_clear_all.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_clear_all.h new file mode 100644 index 0000000..1209868 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_clear_all.h @@ -0,0 +1,233 @@ +// MESSAGE MISSION_CLEAR_ALL PACKING + +#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL 45 + +typedef struct __mavlink_mission_clear_all_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_clear_all_t; + +#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_ID_45_LEN 2 + +#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC 232 +#define MAVLINK_MSG_ID_45_CRC 232 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ + "MISSION_CLEAR_ALL", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_clear_all message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#else + mavlink_mission_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +} + +/** + * @brief Pack a mission_clear_all message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#else + mavlink_mission_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +} + +/** + * @brief Encode a mission_clear_all struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); +} + +/** + * @brief Encode a mission_clear_all struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); +} + +/** + * @brief Send a mission_clear_all message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#else + mavlink_mission_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#else + mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_CLEAR_ALL UNPACKING + + +/** + * @brief Get field target_system from mission_clear_all message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_clear_all_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mission_clear_all message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a mission_clear_all message into a struct + * + * @param msg The message to decode + * @param mission_clear_all C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* msg, mavlink_mission_clear_all_t* mission_clear_all) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg); + mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg); +#else + memcpy(mission_clear_all, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_count.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_count.h new file mode 100644 index 0000000..7e4748e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_count.h @@ -0,0 +1,257 @@ +// MESSAGE MISSION_COUNT PACKING + +#define MAVLINK_MSG_ID_MISSION_COUNT 44 + +typedef struct __mavlink_mission_count_t +{ + uint16_t count; ///< Number of mission items in the sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_count_t; + +#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 4 +#define MAVLINK_MSG_ID_44_LEN 4 + +#define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221 +#define MAVLINK_MSG_ID_44_CRC 221 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ + "MISSION_COUNT", \ + 3, \ + { { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_count message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param count Number of mission items in the sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#else + mavlink_mission_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +} + +/** + * @brief Pack a mission_count message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param count Number of mission items in the sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#else + mavlink_mission_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +} + +/** + * @brief Encode a mission_count struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count); +} + +/** + * @brief Encode a mission_count struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); +} + +/** + * @brief Send a mission_count message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param count Number of mission items in the sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#else + mavlink_mission_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#else + mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf; + packet->count = count; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_COUNT UNPACKING + + +/** + * @brief Get field target_system from mission_count message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_count_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from mission_count message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_count_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field count from mission_count message + * + * @return Number of mission items in the sequence + */ +static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a mission_count message into a struct + * + * @param msg The message to decode + * @param mission_count C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg, mavlink_mission_count_t* mission_count) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_count->count = mavlink_msg_mission_count_get_count(msg); + mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg); + mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg); +#else + memcpy(mission_count, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_current.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_current.h new file mode 100644 index 0000000..201b7a6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_current.h @@ -0,0 +1,209 @@ +// MESSAGE MISSION_CURRENT PACKING + +#define MAVLINK_MSG_ID_MISSION_CURRENT 42 + +typedef struct __mavlink_mission_current_t +{ + uint16_t seq; ///< Sequence +} mavlink_mission_current_t; + +#define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 +#define MAVLINK_MSG_ID_42_LEN 2 + +#define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28 +#define MAVLINK_MSG_ID_42_CRC 28 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ + "MISSION_CURRENT", \ + 1, \ + { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ + } \ +} + + +/** + * @brief Pack a mission_current message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#else + mavlink_mission_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +} + +/** + * @brief Pack a mission_current message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#else + mavlink_mission_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +} + +/** + * @brief Encode a mission_current struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); +} + +/** + * @brief Encode a mission_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); +} + +/** + * @brief Send a mission_current message + * @param chan MAVLink channel to send the message + * + * @param seq Sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#else + mavlink_mission_current_t packet; + packet.seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#else + mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_CURRENT UNPACKING + + +/** + * @brief Get field seq from mission_current message + * + * @return Sequence + */ +static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a mission_current message into a struct + * + * @param msg The message to decode + * @param mission_current C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_current->seq = mavlink_msg_mission_current_get_seq(msg); +#else + memcpy(mission_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_item.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item.h new file mode 100644 index 0000000..ef9394f --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item.h @@ -0,0 +1,521 @@ +// MESSAGE MISSION_ITEM PACKING + +#define MAVLINK_MSG_ID_MISSION_ITEM 39 + +typedef struct __mavlink_mission_item_t +{ + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum + float x; ///< PARAM5 / local: x position, global: latitude + float y; ///< PARAM6 / y position: global: longitude + float z; ///< PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + uint16_t seq; ///< Sequence + uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp +} mavlink_mission_item_t; + +#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37 +#define MAVLINK_MSG_ID_39_LEN 37 + +#define MAVLINK_MSG_ID_MISSION_ITEM_CRC 254 +#define MAVLINK_MSG_ID_39_CRC 254 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ + "MISSION_ITEM", \ + 14, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mission_item_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mission_item_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mission_item_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mission_item_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_t, autocontinue) }, \ + } \ +} + + +/** + * @brief Pack a mission_item message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#else + mavlink_mission_item_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +} + +/** + * @brief Pack a mission_item message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#else + mavlink_mission_item_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +} + +/** + * @brief Encode a mission_item struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); +} + +/** + * @brief Encode a mission_item struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); +} + +/** + * @brief Send a mission_item message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude + * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#else + mavlink_mission_item_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#else + mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_ITEM UNPACKING + + +/** + * @brief Get field target_system from mission_item message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_item_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from mission_item message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_item_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field seq from mission_item message + * + * @return Sequence + */ +static inline uint16_t mavlink_msg_mission_item_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field frame from mission_item message + * + * @return The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + */ +static inline uint8_t mavlink_msg_mission_item_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field command from mission_item message + * + * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + */ +static inline uint16_t mavlink_msg_mission_item_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field current from mission_item message + * + * @return false:0, true:1 + */ +static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field autocontinue from mission_item message + * + * @return autocontinue to next wp + */ +static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field param1 from mission_item message + * + * @return PARAM1, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from mission_item message + * + * @return PARAM2, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from mission_item message + * + * @return PARAM3, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from mission_item message + * + * @return PARAM4, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field x from mission_item message + * + * @return PARAM5 / local: x position, global: latitude + */ +static inline float mavlink_msg_mission_item_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field y from mission_item message + * + * @return PARAM6 / y position: global: longitude + */ +static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field z from mission_item message + * + * @return PARAM7 / z position: global: altitude (relative or absolute, depending on frame. + */ +static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a mission_item message into a struct + * + * @param msg The message to decode + * @param mission_item C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mavlink_mission_item_t* mission_item) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_item->param1 = mavlink_msg_mission_item_get_param1(msg); + mission_item->param2 = mavlink_msg_mission_item_get_param2(msg); + mission_item->param3 = mavlink_msg_mission_item_get_param3(msg); + mission_item->param4 = mavlink_msg_mission_item_get_param4(msg); + mission_item->x = mavlink_msg_mission_item_get_x(msg); + mission_item->y = mavlink_msg_mission_item_get_y(msg); + mission_item->z = mavlink_msg_mission_item_get_z(msg); + mission_item->seq = mavlink_msg_mission_item_get_seq(msg); + mission_item->command = mavlink_msg_mission_item_get_command(msg); + mission_item->target_system = mavlink_msg_mission_item_get_target_system(msg); + mission_item->target_component = mavlink_msg_mission_item_get_target_component(msg); + mission_item->frame = mavlink_msg_mission_item_get_frame(msg); + mission_item->current = mavlink_msg_mission_item_get_current(msg); + mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg); +#else + memcpy(mission_item, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_int.h new file mode 100644 index 0000000..58d102e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_int.h @@ -0,0 +1,521 @@ +// MESSAGE MISSION_ITEM_INT PACKING + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT 73 + +typedef struct __mavlink_mission_item_int_t +{ + float param1; ///< PARAM1, see MAV_CMD enum + float param2; ///< PARAM2, see MAV_CMD enum + float param3; ///< PARAM3, see MAV_CMD enum + float param4; ///< PARAM4, see MAV_CMD enum + int32_t x; ///< PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + int32_t y; ///< PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + float z; ///< PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + uint16_t seq; ///< Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp +} mavlink_mission_item_int_t; + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 37 +#define MAVLINK_MSG_ID_73_LEN 37 + +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC 38 +#define MAVLINK_MSG_ID_73_CRC 38 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ + "MISSION_ITEM_INT", \ + 14, \ + { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mission_item_int_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mission_item_int_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mission_item_int_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mission_item_int_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_int_t, command) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_int_t, frame) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_int_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_int_t, autocontinue) }, \ + } \ +} + + +/** + * @brief Pack a mission_item_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} + +/** + * @brief Pack a mission_item_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} + +/** + * @brief Encode a mission_item_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_item_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) +{ + return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); +} + +/** + * @brief Encode a mission_item_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) +{ + return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); +} + +/** + * @brief Send a mission_item_int message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#else + mavlink_mission_item_int_t *packet = (mavlink_mission_item_int_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_ITEM_INT UNPACKING + + +/** + * @brief Get field target_system from mission_item_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_item_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from mission_item_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_item_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field seq from mission_item_int message + * + * @return Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + */ +static inline uint16_t mavlink_msg_mission_item_int_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field frame from mission_item_int message + * + * @return The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h + */ +static inline uint8_t mavlink_msg_mission_item_int_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field command from mission_item_int message + * + * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs + */ +static inline uint16_t mavlink_msg_mission_item_int_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field current from mission_item_int message + * + * @return false:0, true:1 + */ +static inline uint8_t mavlink_msg_mission_item_int_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field autocontinue from mission_item_int message + * + * @return autocontinue to next wp + */ +static inline uint8_t mavlink_msg_mission_item_int_get_autocontinue(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field param1 from mission_item_int message + * + * @return PARAM1, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param2 from mission_item_int message + * + * @return PARAM2, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param3 from mission_item_int message + * + * @return PARAM3, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param4 from mission_item_int message + * + * @return PARAM4, see MAV_CMD enum + */ +static inline float mavlink_msg_mission_item_int_get_param4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field x from mission_item_int message + * + * @return PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + */ +static inline int32_t mavlink_msg_mission_item_int_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field y from mission_item_int message + * + * @return PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + */ +static inline int32_t mavlink_msg_mission_item_int_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field z from mission_item_int message + * + * @return PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + */ +static inline float mavlink_msg_mission_item_int_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a mission_item_int message into a struct + * + * @param msg The message to decode + * @param mission_item_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_item_int_decode(const mavlink_message_t* msg, mavlink_mission_item_int_t* mission_item_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_item_int->param1 = mavlink_msg_mission_item_int_get_param1(msg); + mission_item_int->param2 = mavlink_msg_mission_item_int_get_param2(msg); + mission_item_int->param3 = mavlink_msg_mission_item_int_get_param3(msg); + mission_item_int->param4 = mavlink_msg_mission_item_int_get_param4(msg); + mission_item_int->x = mavlink_msg_mission_item_int_get_x(msg); + mission_item_int->y = mavlink_msg_mission_item_int_get_y(msg); + mission_item_int->z = mavlink_msg_mission_item_int_get_z(msg); + mission_item_int->seq = mavlink_msg_mission_item_int_get_seq(msg); + mission_item_int->command = mavlink_msg_mission_item_int_get_command(msg); + mission_item_int->target_system = mavlink_msg_mission_item_int_get_target_system(msg); + mission_item_int->target_component = mavlink_msg_mission_item_int_get_target_component(msg); + mission_item_int->frame = mavlink_msg_mission_item_int_get_frame(msg); + mission_item_int->current = mavlink_msg_mission_item_int_get_current(msg); + mission_item_int->autocontinue = mavlink_msg_mission_item_int_get_autocontinue(msg); +#else + memcpy(mission_item_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_reached.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_reached.h new file mode 100644 index 0000000..9dfa280 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_item_reached.h @@ -0,0 +1,209 @@ +// MESSAGE MISSION_ITEM_REACHED PACKING + +#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46 + +typedef struct __mavlink_mission_item_reached_t +{ + uint16_t seq; ///< Sequence +} mavlink_mission_item_reached_t; + +#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN 2 +#define MAVLINK_MSG_ID_46_LEN 2 + +#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC 11 +#define MAVLINK_MSG_ID_46_CRC 11 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \ + "MISSION_ITEM_REACHED", \ + 1, \ + { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \ + } \ +} + + +/** + * @brief Pack a mission_item_reached message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#else + mavlink_mission_item_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +} + +/** + * @brief Pack a mission_item_reached message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#else + mavlink_mission_item_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +} + +/** + * @brief Encode a mission_item_reached struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq); +} + +/** + * @brief Encode a mission_item_reached struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); +} + +/** + * @brief Send a mission_item_reached message + * @param chan MAVLink channel to send the message + * + * @param seq Sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#else + mavlink_mission_item_reached_t packet; + packet.seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#else + mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_ITEM_REACHED UNPACKING + + +/** + * @brief Get field seq from mission_item_reached message + * + * @return Sequence + */ +static inline uint16_t mavlink_msg_mission_item_reached_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a mission_item_reached message into a struct + * + * @param msg The message to decode + * @param mission_item_reached C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message_t* msg, mavlink_mission_item_reached_t* mission_item_reached) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_item_reached->seq = mavlink_msg_mission_item_reached_get_seq(msg); +#else + memcpy(mission_item_reached, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_request.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request.h new file mode 100644 index 0000000..29b0ef6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request.h @@ -0,0 +1,257 @@ +// MESSAGE MISSION_REQUEST PACKING + +#define MAVLINK_MSG_ID_MISSION_REQUEST 40 + +typedef struct __mavlink_mission_request_t +{ + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_request_t; + +#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 4 +#define MAVLINK_MSG_ID_40_LEN 4 + +#define MAVLINK_MSG_ID_MISSION_REQUEST_CRC 230 +#define MAVLINK_MSG_ID_40_CRC 230 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ + "MISSION_REQUEST", \ + 3, \ + { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#else + mavlink_mission_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a mission_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#else + mavlink_mission_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a mission_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); +} + +/** + * @brief Encode a mission_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); +} + +/** + * @brief Send a mission_request message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#else + mavlink_mission_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#else + mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_REQUEST UNPACKING + + +/** + * @brief Get field target_system from mission_request message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_request_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from mission_request message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_request_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field seq from mission_request message + * + * @return Sequence + */ +static inline uint16_t mavlink_msg_mission_request_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a mission_request message into a struct + * + * @param msg The message to decode + * @param mission_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* msg, mavlink_mission_request_t* mission_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_request->seq = mavlink_msg_mission_request_get_seq(msg); + mission_request->target_system = mavlink_msg_mission_request_get_target_system(msg); + mission_request->target_component = mavlink_msg_mission_request_get_target_component(msg); +#else + memcpy(mission_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_list.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_list.h new file mode 100644 index 0000000..a275348 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_list.h @@ -0,0 +1,233 @@ +// MESSAGE MISSION_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST 43 + +typedef struct __mavlink_mission_request_list_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_request_list_t; + +#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_43_LEN 2 + +#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC 132 +#define MAVLINK_MSG_ID_43_CRC 132 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ + "MISSION_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#else + mavlink_mission_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Pack a mission_request_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#else + mavlink_mission_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Encode a mission_request_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); +} + +/** + * @brief Encode a mission_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); +} + +/** + * @brief Send a mission_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#else + mavlink_mission_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#else + mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from mission_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mission_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a mission_request_list message into a struct + * + * @param msg The message to decode + * @param mission_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_request_list_decode(const mavlink_message_t* msg, mavlink_mission_request_list_t* mission_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg); + mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg); +#else + memcpy(mission_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_partial_list.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_partial_list.h new file mode 100644 index 0000000..79a88dc --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_request_partial_list.h @@ -0,0 +1,281 @@ +// MESSAGE MISSION_REQUEST_PARTIAL_LIST PACKING + +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST 37 + +typedef struct __mavlink_mission_request_partial_list_t +{ + int16_t start_index; ///< Start index, 0 by default + int16_t end_index; ///< End index, -1 by default (-1: send list to end). Else a valid index of the list + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_request_partial_list_t; + +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_37_LEN 6 + +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC 212 +#define MAVLINK_MSG_ID_37_CRC 212 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ + "MISSION_REQUEST_PARTIAL_LIST", \ + 4, \ + { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ + { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_request_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +} + +/** + * @brief Pack a mission_request_partial_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +} + +/** + * @brief Encode a mission_request_partial_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + +/** + * @brief Encode a mission_request_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + +/** + * @brief Send a mission_request_partial_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING + + +/** + * @brief Get field target_system from mission_request_partial_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from mission_request_partial_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start_index from mission_request_partial_list message + * + * @return Start index, 0 by default + */ +static inline int16_t mavlink_msg_mission_request_partial_list_get_start_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field end_index from mission_request_partial_list message + * + * @return End index, -1 by default (-1: send list to end). Else a valid index of the list + */ +static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Decode a mission_request_partial_list message into a struct + * + * @param msg The message to decode + * @param mission_request_partial_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_request_partial_list->start_index = mavlink_msg_mission_request_partial_list_get_start_index(msg); + mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg); + mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg); + mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg); +#else + memcpy(mission_request_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_set_current.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_set_current.h new file mode 100644 index 0000000..0c2a3ad --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_set_current.h @@ -0,0 +1,257 @@ +// MESSAGE MISSION_SET_CURRENT PACKING + +#define MAVLINK_MSG_ID_MISSION_SET_CURRENT 41 + +typedef struct __mavlink_mission_set_current_t +{ + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_set_current_t; + +#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN 4 +#define MAVLINK_MSG_ID_41_LEN 4 + +#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC 28 +#define MAVLINK_MSG_ID_41_CRC 28 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT { \ + "MISSION_SET_CURRENT", \ + 3, \ + { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_set_current_t, seq) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_set_current_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_set_current_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_set_current message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#else + mavlink_mission_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +} + +/** + * @brief Pack a mission_set_current message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#else + mavlink_mission_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +} + +/** + * @brief Encode a mission_set_current struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack(system_id, component_id, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + +/** + * @brief Encode a mission_set_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + +/** + * @brief Send a mission_set_current message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#else + mavlink_mission_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#else + mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_SET_CURRENT UNPACKING + + +/** + * @brief Get field target_system from mission_set_current message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_set_current_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from mission_set_current message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_set_current_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field seq from mission_set_current message + * + * @return Sequence + */ +static inline uint16_t mavlink_msg_mission_set_current_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a mission_set_current message into a struct + * + * @param msg The message to decode + * @param mission_set_current C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_set_current_decode(const mavlink_message_t* msg, mavlink_mission_set_current_t* mission_set_current) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_set_current->seq = mavlink_msg_mission_set_current_get_seq(msg); + mission_set_current->target_system = mavlink_msg_mission_set_current_get_target_system(msg); + mission_set_current->target_component = mavlink_msg_mission_set_current_get_target_component(msg); +#else + memcpy(mission_set_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_mission_write_partial_list.h b/flightcode/mavlink/c_library/common/mavlink_msg_mission_write_partial_list.h new file mode 100644 index 0000000..17a5a6b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_mission_write_partial_list.h @@ -0,0 +1,281 @@ +// MESSAGE MISSION_WRITE_PARTIAL_LIST PACKING + +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST 38 + +typedef struct __mavlink_mission_write_partial_list_t +{ + int16_t start_index; ///< Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + int16_t end_index; ///< End index, equal or greater than start index. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_mission_write_partial_list_t; + +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_38_LEN 6 + +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC 9 +#define MAVLINK_MSG_ID_38_CRC 9 + + + +#define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ + "MISSION_WRITE_PARTIAL_LIST", \ + 4, \ + { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ + { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a mission_write_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +} + +/** + * @brief Pack a mission_write_partial_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +} + +/** + * @brief Encode a mission_write_partial_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + +/** + * @brief Encode a mission_write_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + +/** + * @brief Send a mission_write_partial_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING + + +/** + * @brief Get field target_system from mission_write_partial_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from mission_write_partial_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field start_index from mission_write_partial_list message + * + * @return Start index, 0 by default and smaller / equal to the largest index of the current onboard list. + */ +static inline int16_t mavlink_msg_mission_write_partial_list_get_start_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field end_index from mission_write_partial_list message + * + * @return End index, equal or greater than start index. + */ +static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Decode a mission_write_partial_list message into a struct + * + * @param msg The message to decode + * @param mission_write_partial_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + mission_write_partial_list->start_index = mavlink_msg_mission_write_partial_list_get_start_index(msg); + mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg); + mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg); + mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg); +#else + memcpy(mission_write_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_named_value_float.h b/flightcode/mavlink/c_library/common/mavlink_msg_named_value_float.h new file mode 100644 index 0000000..df82704 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_named_value_float.h @@ -0,0 +1,249 @@ +// MESSAGE NAMED_VALUE_FLOAT PACKING + +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 251 + +typedef struct __mavlink_named_value_float_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float value; ///< Floating point value + char name[10]; ///< Name of the debug variable +} mavlink_named_value_float_t; + +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 18 +#define MAVLINK_MSG_ID_251_LEN 18 + +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC 170 +#define MAVLINK_MSG_ID_251_CRC 170 + +#define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT { \ + "NAMED_VALUE_FLOAT", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_float_t, time_boot_ms) }, \ + { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_named_value_float_t, value) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_float_t, name) }, \ + } \ +} + + +/** + * @brief Pack a named_value_float message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Floating point value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +} + +/** + * @brief Pack a named_value_float message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Floating point value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const char *name,float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +} + +/** + * @brief Encode a named_value_float struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + +/** + * @brief Encode a named_value_float struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + +/** + * @brief Send a named_value_float message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Floating point value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#else + mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE NAMED_VALUE_FLOAT UNPACKING + + +/** + * @brief Get field time_boot_ms from named_value_float message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_named_value_float_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field name from named_value_float message + * + * @return Name of the debug variable + */ +static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 8); +} + +/** + * @brief Get field value from named_value_float message + * + * @return Floating point value + */ +static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Decode a named_value_float message into a struct + * + * @param msg The message to decode + * @param named_value_float C-struct to decode the message contents into + */ +static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float) +{ +#if MAVLINK_NEED_BYTE_SWAP + named_value_float->time_boot_ms = mavlink_msg_named_value_float_get_time_boot_ms(msg); + named_value_float->value = mavlink_msg_named_value_float_get_value(msg); + mavlink_msg_named_value_float_get_name(msg, named_value_float->name); +#else + memcpy(named_value_float, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_named_value_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_named_value_int.h new file mode 100644 index 0000000..cfdd73d --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_named_value_int.h @@ -0,0 +1,249 @@ +// MESSAGE NAMED_VALUE_INT PACKING + +#define MAVLINK_MSG_ID_NAMED_VALUE_INT 252 + +typedef struct __mavlink_named_value_int_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int32_t value; ///< Signed integer value + char name[10]; ///< Name of the debug variable +} mavlink_named_value_int_t; + +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 18 +#define MAVLINK_MSG_ID_252_LEN 18 + +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC 44 +#define MAVLINK_MSG_ID_252_CRC 44 + +#define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT { \ + "NAMED_VALUE_INT", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_int_t, time_boot_ms) }, \ + { "value", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_named_value_int_t, value) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_int_t, name) }, \ + } \ +} + + +/** + * @brief Pack a named_value_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Signed integer value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +} + +/** + * @brief Pack a named_value_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Signed integer value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const char *name,int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +} + +/** + * @brief Encode a named_value_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + +/** + * @brief Encode a named_value_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + +/** + * @brief Send a named_value_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param name Name of the debug variable + * @param value Signed integer value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#else + mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE NAMED_VALUE_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from named_value_int message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_named_value_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field name from named_value_int message + * + * @return Name of the debug variable + */ +static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 8); +} + +/** + * @brief Get field value from named_value_int message + * + * @return Signed integer value + */ +static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Decode a named_value_int message into a struct + * + * @param msg The message to decode + * @param named_value_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + named_value_int->time_boot_ms = mavlink_msg_named_value_int_get_time_boot_ms(msg); + named_value_int->value = mavlink_msg_named_value_int_get_value(msg); + mavlink_msg_named_value_int_get_name(msg, named_value_int->name); +#else + memcpy(named_value_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_nav_controller_output.h b/flightcode/mavlink/c_library/common/mavlink_msg_nav_controller_output.h new file mode 100644 index 0000000..b9a748b --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_nav_controller_output.h @@ -0,0 +1,377 @@ +// MESSAGE NAV_CONTROLLER_OUTPUT PACKING + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 + +typedef struct __mavlink_nav_controller_output_t +{ + float nav_roll; ///< Current desired roll in degrees + float nav_pitch; ///< Current desired pitch in degrees + float alt_error; ///< Current altitude error in meters + float aspd_error; ///< Current airspeed error in meters/second + float xtrack_error; ///< Current crosstrack error on x-y plane in meters + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current MISSION/target in degrees + uint16_t wp_dist; ///< Distance to active MISSION in meters +} mavlink_nav_controller_output_t; + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_ID_62_LEN 26 + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC 183 +#define MAVLINK_MSG_ID_62_CRC 183 + + + +#define MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT { \ + "NAV_CONTROLLER_OUTPUT", \ + 8, \ + { { "nav_roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \ + { "nav_pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \ + { "alt_error", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_nav_controller_output_t, alt_error) }, \ + { "aspd_error", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \ + { "xtrack_error", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \ + { "nav_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \ + { "target_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \ + { "wp_dist", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \ + } \ +} + + +/** + * @brief Pack a nav_controller_output message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current MISSION/target in degrees + * @param wp_dist Distance to active MISSION in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +} + +/** + * @brief Pack a nav_controller_output message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current MISSION/target in degrees + * @param wp_dist Distance to active MISSION in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float nav_roll,float nav_pitch,int16_t nav_bearing,int16_t target_bearing,uint16_t wp_dist,float alt_error,float aspd_error,float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +} + +/** + * @brief Encode a nav_controller_output struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + +/** + * @brief Encode a nav_controller_output struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + +/** + * @brief Send a nav_controller_output message + * @param chan MAVLink channel to send the message + * + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current MISSION/target in degrees + * @param wp_dist Distance to active MISSION in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#else + mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf; + packet->nav_roll = nav_roll; + packet->nav_pitch = nav_pitch; + packet->alt_error = alt_error; + packet->aspd_error = aspd_error; + packet->xtrack_error = xtrack_error; + packet->nav_bearing = nav_bearing; + packet->target_bearing = target_bearing; + packet->wp_dist = wp_dist; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING + + +/** + * @brief Get field nav_roll from nav_controller_output message + * + * @return Current desired roll in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field nav_pitch from nav_controller_output message + * + * @return Current desired pitch in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field nav_bearing from nav_controller_output message + * + * @return Current desired heading in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field target_bearing from nav_controller_output message + * + * @return Bearing to current MISSION/target in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field wp_dist from nav_controller_output message + * + * @return Distance to active MISSION in meters + */ +static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field alt_error from nav_controller_output message + * + * @return Current altitude error in meters + */ +static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field aspd_error from nav_controller_output message + * + * @return Current airspeed error in meters/second + */ +static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field xtrack_error from nav_controller_output message + * + * @return Current crosstrack error on x-y plane in meters + */ +static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a nav_controller_output message into a struct + * + * @param msg The message to decode + * @param nav_controller_output C-struct to decode the message contents into + */ +static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) +{ +#if MAVLINK_NEED_BYTE_SWAP + nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); + nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); + nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); + nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); + nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); + nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); + nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); + nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); +#else + memcpy(nav_controller_output, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow.h b/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow.h new file mode 100644 index 0000000..6a2efc6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow.h @@ -0,0 +1,377 @@ +// MESSAGE OPTICAL_FLOW PACKING + +#define MAVLINK_MSG_ID_OPTICAL_FLOW 100 + +typedef struct __mavlink_optical_flow_t +{ + uint64_t time_usec; ///< Timestamp (UNIX) + float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated + float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated + float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) + int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality +} mavlink_optical_flow_t; + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 26 +#define MAVLINK_MSG_ID_100_LEN 26 + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_CRC 175 +#define MAVLINK_MSG_ID_100_CRC 175 + + + +#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ + "OPTICAL_FLOW", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ + { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, flow_comp_m_x) }, \ + { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ + { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ + { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_optical_flow_t, flow_y) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ + } \ +} + + +/** + * @brief Pack a optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Pack a optical_flow message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Encode a optical_flow struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** + * @brief Encode a optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** + * @brief Send a optical_flow message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE OPTICAL_FLOW UNPACKING + + +/** + * @brief Get field time_usec from optical_flow message + * + * @return Timestamp (UNIX) + */ +static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from optical_flow message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field flow_x from optical_flow message + * + * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels) + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field flow_y from optical_flow message + * + * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels) + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field flow_comp_m_x from optical_flow message + * + * @return Flow in meters in x-sensor direction, angular-speed compensated + */ +static inline float mavlink_msg_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field flow_comp_m_y from optical_flow message + * + * @return Flow in meters in y-sensor direction, angular-speed compensated + */ +static inline float mavlink_msg_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field quality from optical_flow message + * + * @return Optical flow quality / confidence. 0: bad, 255: maximum quality + */ +static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field ground_distance from optical_flow message + * + * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + */ +static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a optical_flow message into a struct + * + * @param msg The message to decode + * @param optical_flow C-struct to decode the message contents into + */ +static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow) +{ +#if MAVLINK_NEED_BYTE_SWAP + optical_flow->time_usec = mavlink_msg_optical_flow_get_time_usec(msg); + optical_flow->flow_comp_m_x = mavlink_msg_optical_flow_get_flow_comp_m_x(msg); + optical_flow->flow_comp_m_y = mavlink_msg_optical_flow_get_flow_comp_m_y(msg); + optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg); + optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg); + optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); + optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); + optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); +#else + memcpy(optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow_rad.h b/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow_rad.h new file mode 100644 index 0000000..427b56a --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_optical_flow_rad.h @@ -0,0 +1,473 @@ +// MESSAGE OPTICAL_FLOW_RAD PACKING + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD 106 + +typedef struct __mavlink_optical_flow_rad_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + float integrated_xgyro; ///< RH rotation around X axis (rad) + float integrated_ygyro; ///< RH rotation around Y axis (rad) + float integrated_zgyro; ///< RH rotation around Z axis (rad) + uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled. + float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality +} mavlink_optical_flow_rad_t; + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN 44 +#define MAVLINK_MSG_ID_106_LEN 44 + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC 138 +#define MAVLINK_MSG_ID_106_CRC 138 + + + +#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD { \ + "OPTICAL_FLOW_RAD", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_rad_t, time_usec) }, \ + { "integration_time_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_optical_flow_rad_t, integration_time_us) }, \ + { "integrated_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_rad_t, integrated_x) }, \ + { "integrated_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_rad_t, integrated_y) }, \ + { "integrated_xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_optical_flow_rad_t, integrated_xgyro) }, \ + { "integrated_ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_optical_flow_rad_t, integrated_ygyro) }, \ + { "integrated_zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_optical_flow_rad_t, integrated_zgyro) }, \ + { "time_delta_distance_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_optical_flow_rad_t, time_delta_distance_us) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_optical_flow_rad_t, distance) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_optical_flow_rad_t, temperature) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_optical_flow_rad_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_optical_flow_rad_t, quality) }, \ + } \ +} + + +/** + * @brief Pack a optical_flow_rad message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} + +/** + * @brief Pack a optical_flow_rad message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_rad_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_id,uint32_t integration_time_us,float integrated_x,float integrated_y,float integrated_xgyro,float integrated_ygyro,float integrated_zgyro,int16_t temperature,uint8_t quality,uint32_t time_delta_distance_us,float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} + +/** + * @brief Encode a optical_flow_rad struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param optical_flow_rad C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_rad_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) +{ + return mavlink_msg_optical_flow_rad_pack(system_id, component_id, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); +} + +/** + * @brief Encode a optical_flow_rad struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow_rad C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_rad_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) +{ + return mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, chan, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); +} + +/** + * @brief Send a optical_flow_rad message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param sensor_id Sensor ID + * @param integration_time_us Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro RH rotation around X axis (rad) + * @param integrated_ygyro RH rotation around Y axis (rad) + * @param integrated_zgyro RH rotation around Z axis (rad) + * @param temperature Temperature * 100 in centi-degrees Celsius + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us Time in microseconds since the distance was sampled. + * @param distance Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_optical_flow_rad_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_rad_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#else + mavlink_optical_flow_rad_t *packet = (mavlink_optical_flow_rad_t *)msgbuf; + packet->time_usec = time_usec; + packet->integration_time_us = integration_time_us; + packet->integrated_x = integrated_x; + packet->integrated_y = integrated_y; + packet->integrated_xgyro = integrated_xgyro; + packet->integrated_ygyro = integrated_ygyro; + packet->integrated_zgyro = integrated_zgyro; + packet->time_delta_distance_us = time_delta_distance_us; + packet->distance = distance; + packet->temperature = temperature; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE OPTICAL_FLOW_RAD UNPACKING + + +/** + * @brief Get field time_usec from optical_flow_rad message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_optical_flow_rad_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from optical_flow_rad message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_optical_flow_rad_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field integration_time_us from optical_flow_rad message + * + * @return Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + */ +static inline uint32_t mavlink_msg_optical_flow_rad_get_integration_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field integrated_x from optical_flow_rad message + * + * @return Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field integrated_y from optical_flow_rad message + * + * @return Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field integrated_xgyro from optical_flow_rad message + * + * @return RH rotation around X axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field integrated_ygyro from optical_flow_rad message + * + * @return RH rotation around Y axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field integrated_zgyro from optical_flow_rad message + * + * @return RH rotation around Z axis (rad) + */ +static inline float mavlink_msg_optical_flow_rad_get_integrated_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field temperature from optical_flow_rad message + * + * @return Temperature * 100 in centi-degrees Celsius + */ +static inline int16_t mavlink_msg_optical_flow_rad_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field quality from optical_flow_rad message + * + * @return Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + */ +static inline uint8_t mavlink_msg_optical_flow_rad_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field time_delta_distance_us from optical_flow_rad message + * + * @return Time in microseconds since the distance was sampled. + */ +static inline uint32_t mavlink_msg_optical_flow_rad_get_time_delta_distance_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 32); +} + +/** + * @brief Get field distance from optical_flow_rad message + * + * @return Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance. + */ +static inline float mavlink_msg_optical_flow_rad_get_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a optical_flow_rad message into a struct + * + * @param msg The message to decode + * @param optical_flow_rad C-struct to decode the message contents into + */ +static inline void mavlink_msg_optical_flow_rad_decode(const mavlink_message_t* msg, mavlink_optical_flow_rad_t* optical_flow_rad) +{ +#if MAVLINK_NEED_BYTE_SWAP + optical_flow_rad->time_usec = mavlink_msg_optical_flow_rad_get_time_usec(msg); + optical_flow_rad->integration_time_us = mavlink_msg_optical_flow_rad_get_integration_time_us(msg); + optical_flow_rad->integrated_x = mavlink_msg_optical_flow_rad_get_integrated_x(msg); + optical_flow_rad->integrated_y = mavlink_msg_optical_flow_rad_get_integrated_y(msg); + optical_flow_rad->integrated_xgyro = mavlink_msg_optical_flow_rad_get_integrated_xgyro(msg); + optical_flow_rad->integrated_ygyro = mavlink_msg_optical_flow_rad_get_integrated_ygyro(msg); + optical_flow_rad->integrated_zgyro = mavlink_msg_optical_flow_rad_get_integrated_zgyro(msg); + optical_flow_rad->time_delta_distance_us = mavlink_msg_optical_flow_rad_get_time_delta_distance_us(msg); + optical_flow_rad->distance = mavlink_msg_optical_flow_rad_get_distance(msg); + optical_flow_rad->temperature = mavlink_msg_optical_flow_rad_get_temperature(msg); + optical_flow_rad->sensor_id = mavlink_msg_optical_flow_rad_get_sensor_id(msg); + optical_flow_rad->quality = mavlink_msg_optical_flow_rad_get_quality(msg); +#else + memcpy(optical_flow_rad, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_param_map_rc.h b/flightcode/mavlink/c_library/common/mavlink_msg_param_map_rc.h new file mode 100644 index 0000000..8f6ee0e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_param_map_rc.h @@ -0,0 +1,393 @@ +// MESSAGE PARAM_MAP_RC PACKING + +#define MAVLINK_MSG_ID_PARAM_MAP_RC 50 + +typedef struct __mavlink_param_map_rc_t +{ + float param_value0; ///< Initial parameter value + float scale; ///< Scale, maps the RC range [-1, 1] to a parameter value + float param_value_min; ///< Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + float param_value_max; ///< Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + uint8_t parameter_rc_channel_index; ///< Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds to a potentiometer-knob on the RC. +} mavlink_param_map_rc_t; + +#define MAVLINK_MSG_ID_PARAM_MAP_RC_LEN 37 +#define MAVLINK_MSG_ID_50_LEN 37 + +#define MAVLINK_MSG_ID_PARAM_MAP_RC_CRC 78 +#define MAVLINK_MSG_ID_50_CRC 78 + +#define MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN 16 + +#define MAVLINK_MESSAGE_INFO_PARAM_MAP_RC { \ + "PARAM_MAP_RC", \ + 9, \ + { { "param_value0", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_map_rc_t, param_value0) }, \ + { "scale", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_param_map_rc_t, scale) }, \ + { "param_value_min", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_param_map_rc_t, param_value_min) }, \ + { "param_value_max", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_param_map_rc_t, param_value_max) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_param_map_rc_t, param_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_param_map_rc_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_param_map_rc_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 20, offsetof(mavlink_param_map_rc_t, param_id) }, \ + { "parameter_rc_channel_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_param_map_rc_t, parameter_rc_channel_index) }, \ + } \ +} + + +/** + * @brief Pack a param_map_rc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + * @param parameter_rc_channel_index Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds to a potentiometer-knob on the RC. + * @param param_value0 Initial parameter value + * @param scale Scale, maps the RC range [-1, 1] to a parameter value + * @param param_value_min Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + * @param param_value_max Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_map_rc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index, uint8_t parameter_rc_channel_index, float param_value0, float scale, float param_value_min, float param_value_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_MAP_RC_LEN]; + _mav_put_float(buf, 0, param_value0); + _mav_put_float(buf, 4, scale); + _mav_put_float(buf, 8, param_value_min); + _mav_put_float(buf, 12, param_value_max); + _mav_put_int16_t(buf, 16, param_index); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + _mav_put_uint8_t(buf, 36, parameter_rc_channel_index); + _mav_put_char_array(buf, 20, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#else + mavlink_param_map_rc_t packet; + packet.param_value0 = param_value0; + packet.scale = scale; + packet.param_value_min = param_value_min; + packet.param_value_max = param_value_max; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.parameter_rc_channel_index = parameter_rc_channel_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_MAP_RC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +} + +/** + * @brief Pack a param_map_rc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + * @param parameter_rc_channel_index Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds to a potentiometer-knob on the RC. + * @param param_value0 Initial parameter value + * @param scale Scale, maps the RC range [-1, 1] to a parameter value + * @param param_value_min Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + * @param param_value_max Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_map_rc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index,uint8_t parameter_rc_channel_index,float param_value0,float scale,float param_value_min,float param_value_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_MAP_RC_LEN]; + _mav_put_float(buf, 0, param_value0); + _mav_put_float(buf, 4, scale); + _mav_put_float(buf, 8, param_value_min); + _mav_put_float(buf, 12, param_value_max); + _mav_put_int16_t(buf, 16, param_index); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + _mav_put_uint8_t(buf, 36, parameter_rc_channel_index); + _mav_put_char_array(buf, 20, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#else + mavlink_param_map_rc_t packet; + packet.param_value0 = param_value0; + packet.scale = scale; + packet.param_value_min = param_value_min; + packet.param_value_max = param_value_max; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.parameter_rc_channel_index = parameter_rc_channel_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_MAP_RC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +} + +/** + * @brief Encode a param_map_rc struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_map_rc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_map_rc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_map_rc_t* param_map_rc) +{ + return mavlink_msg_param_map_rc_pack(system_id, component_id, msg, param_map_rc->target_system, param_map_rc->target_component, param_map_rc->param_id, param_map_rc->param_index, param_map_rc->parameter_rc_channel_index, param_map_rc->param_value0, param_map_rc->scale, param_map_rc->param_value_min, param_map_rc->param_value_max); +} + +/** + * @brief Encode a param_map_rc struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_map_rc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_map_rc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_map_rc_t* param_map_rc) +{ + return mavlink_msg_param_map_rc_pack_chan(system_id, component_id, chan, msg, param_map_rc->target_system, param_map_rc->target_component, param_map_rc->param_id, param_map_rc->param_index, param_map_rc->parameter_rc_channel_index, param_map_rc->param_value0, param_map_rc->scale, param_map_rc->param_value_min, param_map_rc->param_value_max); +} + +/** + * @brief Send a param_map_rc message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + * @param parameter_rc_channel_index Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds to a potentiometer-knob on the RC. + * @param param_value0 Initial parameter value + * @param scale Scale, maps the RC range [-1, 1] to a parameter value + * @param param_value_min Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + * @param param_value_max Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_map_rc_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index, uint8_t parameter_rc_channel_index, float param_value0, float scale, float param_value_min, float param_value_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_MAP_RC_LEN]; + _mav_put_float(buf, 0, param_value0); + _mav_put_float(buf, 4, scale); + _mav_put_float(buf, 8, param_value_min); + _mav_put_float(buf, 12, param_value_max); + _mav_put_int16_t(buf, 16, param_index); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + _mav_put_uint8_t(buf, 36, parameter_rc_channel_index); + _mav_put_char_array(buf, 20, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +#else + mavlink_param_map_rc_t packet; + packet.param_value0 = param_value0; + packet.scale = scale; + packet.param_value_min = param_value_min; + packet.param_value_max = param_value_max; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.parameter_rc_channel_index = parameter_rc_channel_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)&packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)&packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PARAM_MAP_RC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_map_rc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index, uint8_t parameter_rc_channel_index, float param_value0, float scale, float param_value_min, float param_value_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value0); + _mav_put_float(buf, 4, scale); + _mav_put_float(buf, 8, param_value_min); + _mav_put_float(buf, 12, param_value_max); + _mav_put_int16_t(buf, 16, param_index); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + _mav_put_uint8_t(buf, 36, parameter_rc_channel_index); + _mav_put_char_array(buf, 20, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +#else + mavlink_param_map_rc_t *packet = (mavlink_param_map_rc_t *)msgbuf; + packet->param_value0 = param_value0; + packet->scale = scale; + packet->param_value_min = param_value_min; + packet->param_value_max = param_value_max; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + packet->parameter_rc_channel_index = parameter_rc_channel_index; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PARAM_MAP_RC UNPACKING + + +/** + * @brief Get field target_system from param_map_rc message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_map_rc_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field target_component from param_map_rc message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_map_rc_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field param_id from param_map_rc message + * + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_map_rc_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 20); +} + +/** + * @brief Get field param_index from param_map_rc message + * + * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + */ +static inline int16_t mavlink_msg_param_map_rc_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field parameter_rc_channel_index from param_map_rc message + * + * @return Index of parameter RC channel. Not equal to the RC channel id. Typically correpsonds to a potentiometer-knob on the RC. + */ +static inline uint8_t mavlink_msg_param_map_rc_get_parameter_rc_channel_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field param_value0 from param_map_rc message + * + * @return Initial parameter value + */ +static inline float mavlink_msg_param_map_rc_get_param_value0(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field scale from param_map_rc message + * + * @return Scale, maps the RC range [-1, 1] to a parameter value + */ +static inline float mavlink_msg_param_map_rc_get_scale(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field param_value_min from param_map_rc message + * + * @return Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + */ +static inline float mavlink_msg_param_map_rc_get_param_value_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field param_value_max from param_map_rc message + * + * @return Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + */ +static inline float mavlink_msg_param_map_rc_get_param_value_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a param_map_rc message into a struct + * + * @param msg The message to decode + * @param param_map_rc C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_map_rc_decode(const mavlink_message_t* msg, mavlink_param_map_rc_t* param_map_rc) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_map_rc->param_value0 = mavlink_msg_param_map_rc_get_param_value0(msg); + param_map_rc->scale = mavlink_msg_param_map_rc_get_scale(msg); + param_map_rc->param_value_min = mavlink_msg_param_map_rc_get_param_value_min(msg); + param_map_rc->param_value_max = mavlink_msg_param_map_rc_get_param_value_max(msg); + param_map_rc->param_index = mavlink_msg_param_map_rc_get_param_index(msg); + param_map_rc->target_system = mavlink_msg_param_map_rc_get_target_system(msg); + param_map_rc->target_component = mavlink_msg_param_map_rc_get_target_component(msg); + mavlink_msg_param_map_rc_get_param_id(msg, param_map_rc->param_id); + param_map_rc->parameter_rc_channel_index = mavlink_msg_param_map_rc_get_parameter_rc_channel_index(msg); +#else + memcpy(param_map_rc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_param_request_list.h b/flightcode/mavlink/c_library/common/mavlink_msg_param_request_list.h new file mode 100644 index 0000000..f9466b0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_param_request_list.h @@ -0,0 +1,233 @@ +// MESSAGE PARAM_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 + +typedef struct __mavlink_param_request_list_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_param_request_list_t; + +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_21_LEN 2 + +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC 159 +#define MAVLINK_MSG_ID_21_CRC 159 + + + +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ + "PARAM_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a param_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Pack a param_request_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +} + +/** + * @brief Encode a param_request_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** + * @brief Encode a param_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** + * @brief Send a param_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#else + mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PARAM_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from param_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a param_request_list message into a struct + * + * @param msg The message to decode + * @param param_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); + param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); +#else + memcpy(param_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_param_request_read.h b/flightcode/mavlink/c_library/common/mavlink_msg_param_request_read.h new file mode 100644 index 0000000..730cff0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_param_request_read.h @@ -0,0 +1,273 @@ +// MESSAGE PARAM_REQUEST_READ PACKING + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 + +typedef struct __mavlink_param_request_read_t +{ + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string +} mavlink_param_request_read_t; + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_ID_20_LEN 20 + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC 214 +#define MAVLINK_MSG_ID_20_CRC 214 + +#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 + +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ + "PARAM_REQUEST_READ", \ + 4, \ + { { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \ + } \ +} + + +/** + * @brief Pack a param_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +} + +/** + * @brief Pack a param_request_read message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +} + +/** + * @brief Encode a param_request_read struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** + * @brief Encode a param_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** + * @brief Send a param_request_read message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#else + mavlink_param_request_read_t *packet = (mavlink_param_request_read_t *)msgbuf; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PARAM_REQUEST_READ UNPACKING + + +/** + * @brief Get field target_system from param_request_read message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from param_request_read message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field param_id from param_request_read message + * + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 4); +} + +/** + * @brief Get field param_index from param_request_read message + * + * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + */ +static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Decode a param_request_read message into a struct + * + * @param msg The message to decode + * @param param_request_read C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); + param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); + param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); + mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); +#else + memcpy(param_request_read, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_param_set.h b/flightcode/mavlink/c_library/common/mavlink_msg_param_set.h new file mode 100644 index 0000000..f669af1 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_param_set.h @@ -0,0 +1,297 @@ +// MESSAGE PARAM_SET PACKING + +#define MAVLINK_MSG_ID_PARAM_SET 23 + +typedef struct __mavlink_param_set_t +{ + float param_value; ///< Onboard parameter value + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. +} mavlink_param_set_t; + +#define MAVLINK_MSG_ID_PARAM_SET_LEN 23 +#define MAVLINK_MSG_ID_23_LEN 23 + +#define MAVLINK_MSG_ID_PARAM_SET_CRC 168 +#define MAVLINK_MSG_ID_23_CRC 168 + +#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 + +#define MAVLINK_MESSAGE_INFO_PARAM_SET { \ + "PARAM_SET", \ + 5, \ + { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \ + } \ +} + + +/** + * @brief Pack a param_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +} + +/** + * @brief Pack a param_set message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +} + +/** + * @brief Encode a param_set struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + +/** + * @brief Encode a param_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + +/** + * @brief Send a param_set message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#else + mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; + packet->param_value = param_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PARAM_SET UNPACKING + + +/** + * @brief Get field target_system from param_set message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from param_set message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field param_id from param_set message + * + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 6); +} + +/** + * @brief Get field param_value from param_set message + * + * @return Onboard parameter value + */ +static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param_type from param_set message + * + * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + */ +static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Decode a param_set message into a struct + * + * @param msg The message to decode + * @param param_set C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_set->param_value = mavlink_msg_param_set_get_param_value(msg); + param_set->target_system = mavlink_msg_param_set_get_target_system(msg); + param_set->target_component = mavlink_msg_param_set_get_target_component(msg); + mavlink_msg_param_set_get_param_id(msg, param_set->param_id); + param_set->param_type = mavlink_msg_param_set_get_param_type(msg); +#else + memcpy(param_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_param_value.h b/flightcode/mavlink/c_library/common/mavlink_msg_param_value.h new file mode 100644 index 0000000..c279579 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_param_value.h @@ -0,0 +1,297 @@ +// MESSAGE PARAM_VALUE PACKING + +#define MAVLINK_MSG_ID_PARAM_VALUE 22 + +typedef struct __mavlink_param_value_t +{ + float param_value; ///< Onboard parameter value + uint16_t param_count; ///< Total number of onboard parameters + uint16_t param_index; ///< Index of this onboard parameter + char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. +} mavlink_param_value_t; + +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25 +#define MAVLINK_MSG_ID_22_LEN 25 + +#define MAVLINK_MSG_ID_PARAM_VALUE_CRC 220 +#define MAVLINK_MSG_ID_22_CRC 220 + +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 + +#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ + "PARAM_VALUE", \ + 5, \ + { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \ + } \ +} + + +/** + * @brief Pack a param_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +} + +/** + * @brief Pack a param_value message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *param_id,float param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +} + +/** + * @brief Encode a param_value struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + +/** + * @brief Encode a param_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + +/** + * @brief Send a param_value message + * @param chan MAVLink channel to send the message + * + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#else + mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; + packet->param_value = param_value; + packet->param_count = param_count; + packet->param_index = param_index; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PARAM_VALUE UNPACKING + + +/** + * @brief Get field param_id from param_value message + * + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 8); +} + +/** + * @brief Get field param_value from param_value message + * + * @return Onboard parameter value + */ +static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field param_type from param_value message + * + * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + */ +static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field param_count from param_value message + * + * @return Total number of onboard parameters + */ +static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field param_index from param_value message + * + * @return Index of this onboard parameter + */ +static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Decode a param_value message into a struct + * + * @param msg The message to decode + * @param param_value C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_value->param_value = mavlink_msg_param_value_get_param_value(msg); + param_value->param_count = mavlink_msg_param_value_get_param_count(msg); + param_value->param_index = mavlink_msg_param_value_get_param_index(msg); + mavlink_msg_param_value_get_param_id(msg, param_value->param_id); + param_value->param_type = mavlink_msg_param_value_get_param_type(msg); +#else + memcpy(param_value, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_ping.h b/flightcode/mavlink/c_library/common/mavlink_msg_ping.h new file mode 100644 index 0000000..6564ed0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_ping.h @@ -0,0 +1,281 @@ +// MESSAGE PING PACKING + +#define MAVLINK_MSG_ID_PING 4 + +typedef struct __mavlink_ping_t +{ + uint64_t time_usec; ///< Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) + uint32_t seq; ///< PING sequence + uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system +} mavlink_ping_t; + +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_ID_4_LEN 14 + +#define MAVLINK_MSG_ID_PING_CRC 237 +#define MAVLINK_MSG_ID_4_CRC 237 + + + +#define MAVLINK_MESSAGE_INFO_PING { \ + "PING", \ + 4, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time_usec) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a ping message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) + * @param seq PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PING_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); +#endif +} + +/** + * @brief Pack a ping message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) + * @param seq PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t seq,uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PING_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); +#endif +} + +/** + * @brief Encode a ping struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + +/** + * @brief Encode a ping struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + +/** + * @brief Send a ping message + * @param chan MAVLink channel to send the message + * + * @param time_usec Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) + * @param seq PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PING_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); +#endif +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, MAVLINK_MSG_ID_PING_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); +#endif +#else + mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf; + packet->time_usec = time_usec; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE PING UNPACKING + + +/** + * @brief Get field time_usec from ping message + * + * @return Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) + */ +static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field seq from ping message + * + * @return PING sequence + */ +static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field target_system from ping message + * + * @return 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + */ +static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field target_component from ping message + * + * @return 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + */ +static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Decode a ping message into a struct + * + * @param msg The message to decode + * @param ping C-struct to decode the message contents into + */ +static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping) +{ +#if MAVLINK_NEED_BYTE_SWAP + ping->time_usec = mavlink_msg_ping_get_time_usec(msg); + ping->seq = mavlink_msg_ping_get_seq(msg); + ping->target_system = mavlink_msg_ping_get_target_system(msg); + ping->target_component = mavlink_msg_ping_get_target_component(msg); +#else + memcpy(ping, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PING_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_position_target_global_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_position_target_global_int.h new file mode 100644 index 0000000..b960146 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_position_target_global_int.h @@ -0,0 +1,521 @@ +// MESSAGE POSITION_TARGET_GLOBAL_INT PACKING + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT 87 + +typedef struct __mavlink_position_target_global_int_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + int32_t lat_int; ///< X Position in WGS84 frame in 1e7 * meters + int32_t lon_int; ///< Y Position in WGS84 frame in 1e7 * meters + float alt; ///< Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +} mavlink_position_target_global_int_t; + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN 51 +#define MAVLINK_MSG_ID_87_LEN 51 + +#define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC 150 +#define MAVLINK_MSG_ID_87_CRC 150 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT { \ + "POSITION_TARGET_GLOBAL_INT", \ + 14, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_position_target_global_int_t, time_boot_ms) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_position_target_global_int_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_position_target_global_int_t, lon_int) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_global_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_position_target_global_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_position_target_global_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_position_target_global_int_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_position_target_global_int_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_position_target_global_int_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_position_target_global_int_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_position_target_global_int_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_position_target_global_int_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_position_target_global_int_t, type_mask) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_position_target_global_int_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_global_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Pack a position_target_global_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_global_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t coordinate_frame,uint16_t type_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Encode a position_target_global_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_global_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_global_int_t* position_target_global_int) +{ + return mavlink_msg_position_target_global_int_pack(system_id, component_id, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); +} + +/** + * @brief Encode a position_target_global_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_global_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_target_global_int_t* position_target_global_int) +{ + return mavlink_msg_position_target_global_int_pack_chan(system_id, component_id, chan, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); +} + +/** + * @brief Send a position_target_global_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_position_target_global_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_target_global_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_position_target_global_int_t *packet = (mavlink_position_target_global_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_int = lat_int; + packet->lon_int = lon_int; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POSITION_TARGET_GLOBAL_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from position_target_global_int message + * + * @return Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + */ +static inline uint32_t mavlink_msg_position_target_global_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field coordinate_frame from position_target_global_int message + * + * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + */ +static inline uint8_t mavlink_msg_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field type_mask from position_target_global_int message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_position_target_global_int_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field lat_int from position_target_global_int message + * + * @return X Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_position_target_global_int_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_int from position_target_global_int message + * + * @return Y Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_position_target_global_int_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from position_target_global_int message + * + * @return Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + */ +static inline float mavlink_msg_position_target_global_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from position_target_global_int message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from position_target_global_int message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from position_target_global_int message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_global_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from position_target_global_int message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from position_target_global_int message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from position_target_global_int message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_global_int_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from position_target_global_int message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_position_target_global_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from position_target_global_int message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_position_target_global_int_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a position_target_global_int message into a struct + * + * @param msg The message to decode + * @param position_target_global_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_position_target_global_int_decode(const mavlink_message_t* msg, mavlink_position_target_global_int_t* position_target_global_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + position_target_global_int->time_boot_ms = mavlink_msg_position_target_global_int_get_time_boot_ms(msg); + position_target_global_int->lat_int = mavlink_msg_position_target_global_int_get_lat_int(msg); + position_target_global_int->lon_int = mavlink_msg_position_target_global_int_get_lon_int(msg); + position_target_global_int->alt = mavlink_msg_position_target_global_int_get_alt(msg); + position_target_global_int->vx = mavlink_msg_position_target_global_int_get_vx(msg); + position_target_global_int->vy = mavlink_msg_position_target_global_int_get_vy(msg); + position_target_global_int->vz = mavlink_msg_position_target_global_int_get_vz(msg); + position_target_global_int->afx = mavlink_msg_position_target_global_int_get_afx(msg); + position_target_global_int->afy = mavlink_msg_position_target_global_int_get_afy(msg); + position_target_global_int->afz = mavlink_msg_position_target_global_int_get_afz(msg); + position_target_global_int->yaw = mavlink_msg_position_target_global_int_get_yaw(msg); + position_target_global_int->yaw_rate = mavlink_msg_position_target_global_int_get_yaw_rate(msg); + position_target_global_int->type_mask = mavlink_msg_position_target_global_int_get_type_mask(msg); + position_target_global_int->coordinate_frame = mavlink_msg_position_target_global_int_get_coordinate_frame(msg); +#else + memcpy(position_target_global_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_position_target_local_ned.h b/flightcode/mavlink/c_library/common/mavlink_msg_position_target_local_ned.h new file mode 100644 index 0000000..d23d445 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_position_target_local_ned.h @@ -0,0 +1,521 @@ +// MESSAGE POSITION_TARGET_LOCAL_NED PACKING + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED 85 + +typedef struct __mavlink_position_target_local_ned_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float x; ///< X Position in NED frame in meters + float y; ///< Y Position in NED frame in meters + float z; ///< Z Position in NED frame in meters (note, altitude is negative in NED) + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 +} mavlink_position_target_local_ned_t; + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN 51 +#define MAVLINK_MSG_ID_85_LEN 51 + +#define MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC 140 +#define MAVLINK_MSG_ID_85_CRC 140 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED { \ + "POSITION_TARGET_LOCAL_NED", \ + 14, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_position_target_local_ned_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_local_ned_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_local_ned_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_local_ned_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_position_target_local_ned_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_position_target_local_ned_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_position_target_local_ned_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_position_target_local_ned_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_position_target_local_ned_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_position_target_local_ned_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_position_target_local_ned_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_position_target_local_ned_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_position_target_local_ned_t, type_mask) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_position_target_local_ned_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_local_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Pack a position_target_local_ned message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_local_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t coordinate_frame,uint16_t type_mask,float x,float y,float z,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Encode a position_target_local_ned struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_local_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_local_ned_t* position_target_local_ned) +{ + return mavlink_msg_position_target_local_ned_pack(system_id, component_id, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); +} + +/** + * @brief Encode a position_target_local_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_local_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_target_local_ned_t* position_target_local_ned) +{ + return mavlink_msg_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); +} + +/** + * @brief Send a position_target_local_ned message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_position_target_local_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_target_local_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_position_target_local_ned_t *packet = (mavlink_position_target_local_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POSITION_TARGET_LOCAL_NED UNPACKING + + +/** + * @brief Get field time_boot_ms from position_target_local_ned message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_position_target_local_ned_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field coordinate_frame from position_target_local_ned message + * + * @return Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + */ +static inline uint8_t mavlink_msg_position_target_local_ned_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field type_mask from position_target_local_ned message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_position_target_local_ned_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field x from position_target_local_ned message + * + * @return X Position in NED frame in meters + */ +static inline float mavlink_msg_position_target_local_ned_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from position_target_local_ned message + * + * @return Y Position in NED frame in meters + */ +static inline float mavlink_msg_position_target_local_ned_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from position_target_local_ned message + * + * @return Z Position in NED frame in meters (note, altitude is negative in NED) + */ +static inline float mavlink_msg_position_target_local_ned_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from position_target_local_ned message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from position_target_local_ned message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from position_target_local_ned message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_position_target_local_ned_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from position_target_local_ned message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from position_target_local_ned message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from position_target_local_ned message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_position_target_local_ned_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from position_target_local_ned message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_position_target_local_ned_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from position_target_local_ned message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_position_target_local_ned_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a position_target_local_ned message into a struct + * + * @param msg The message to decode + * @param position_target_local_ned C-struct to decode the message contents into + */ +static inline void mavlink_msg_position_target_local_ned_decode(const mavlink_message_t* msg, mavlink_position_target_local_ned_t* position_target_local_ned) +{ +#if MAVLINK_NEED_BYTE_SWAP + position_target_local_ned->time_boot_ms = mavlink_msg_position_target_local_ned_get_time_boot_ms(msg); + position_target_local_ned->x = mavlink_msg_position_target_local_ned_get_x(msg); + position_target_local_ned->y = mavlink_msg_position_target_local_ned_get_y(msg); + position_target_local_ned->z = mavlink_msg_position_target_local_ned_get_z(msg); + position_target_local_ned->vx = mavlink_msg_position_target_local_ned_get_vx(msg); + position_target_local_ned->vy = mavlink_msg_position_target_local_ned_get_vy(msg); + position_target_local_ned->vz = mavlink_msg_position_target_local_ned_get_vz(msg); + position_target_local_ned->afx = mavlink_msg_position_target_local_ned_get_afx(msg); + position_target_local_ned->afy = mavlink_msg_position_target_local_ned_get_afy(msg); + position_target_local_ned->afz = mavlink_msg_position_target_local_ned_get_afz(msg); + position_target_local_ned->yaw = mavlink_msg_position_target_local_ned_get_yaw(msg); + position_target_local_ned->yaw_rate = mavlink_msg_position_target_local_ned_get_yaw_rate(msg); + position_target_local_ned->type_mask = mavlink_msg_position_target_local_ned_get_type_mask(msg); + position_target_local_ned->coordinate_frame = mavlink_msg_position_target_local_ned_get_coordinate_frame(msg); +#else + memcpy(position_target_local_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_power_status.h b/flightcode/mavlink/c_library/common/mavlink_msg_power_status.h new file mode 100644 index 0000000..1a6259a --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_power_status.h @@ -0,0 +1,257 @@ +// MESSAGE POWER_STATUS PACKING + +#define MAVLINK_MSG_ID_POWER_STATUS 125 + +typedef struct __mavlink_power_status_t +{ + uint16_t Vcc; ///< 5V rail voltage in millivolts + uint16_t Vservo; ///< servo rail voltage in millivolts + uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum) +} mavlink_power_status_t; + +#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6 +#define MAVLINK_MSG_ID_125_LEN 6 + +#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203 +#define MAVLINK_MSG_ID_125_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \ + "POWER_STATUS", \ + 3, \ + { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \ + { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a power_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Pack a power_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Vcc,uint16_t Vservo,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Encode a power_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Encode a power_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Send a power_status message + * @param chan MAVLink channel to send the message + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf; + packet->Vcc = Vcc; + packet->Vservo = Vservo; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POWER_STATUS UNPACKING + + +/** + * @brief Get field Vcc from power_status message + * + * @return 5V rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field Vservo from power_status message + * + * @return servo rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field flags from power_status message + * + * @return power supply status flags (see MAV_POWER_STATUS enum) + */ +static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a power_status message into a struct + * + * @param msg The message to decode + * @param power_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg); + power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg); + power_status->flags = mavlink_msg_power_status_get_flags(msg); +#else + memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_radio_status.h b/flightcode/mavlink/c_library/common/mavlink_msg_radio_status.h new file mode 100644 index 0000000..dbd187e --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_radio_status.h @@ -0,0 +1,353 @@ +// MESSAGE RADIO_STATUS PACKING + +#define MAVLINK_MSG_ID_RADIO_STATUS 109 + +typedef struct __mavlink_radio_status_t +{ + uint16_t rxerrors; ///< Receive errors + uint16_t fixed; ///< Count of error corrected packets + uint8_t rssi; ///< Local signal strength + uint8_t remrssi; ///< Remote signal strength + uint8_t txbuf; ///< Remaining free buffer space in percent. + uint8_t noise; ///< Background noise level + uint8_t remnoise; ///< Remote background noise level +} mavlink_radio_status_t; + +#define MAVLINK_MSG_ID_RADIO_STATUS_LEN 9 +#define MAVLINK_MSG_ID_109_LEN 9 + +#define MAVLINK_MSG_ID_RADIO_STATUS_CRC 185 +#define MAVLINK_MSG_ID_109_CRC 185 + + + +#define MAVLINK_MESSAGE_INFO_RADIO_STATUS { \ + "RADIO_STATUS", \ + 7, \ + { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_status_t, rxerrors) }, \ + { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_status_t, fixed) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_status_t, rssi) }, \ + { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_status_t, remrssi) }, \ + { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_status_t, txbuf) }, \ + { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_status_t, noise) }, \ + { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_status_t, remnoise) }, \ + } \ +} + + +/** + * @brief Pack a radio_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param rssi Local signal strength + * @param remrssi Remote signal strength + * @param txbuf Remaining free buffer space in percent. + * @param noise Background noise level + * @param remnoise Remote background noise level + * @param rxerrors Receive errors + * @param fixed Count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} + +/** + * @brief Pack a radio_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rssi Local signal strength + * @param remrssi Remote signal strength + * @param txbuf Remaining free buffer space in percent. + * @param noise Background noise level + * @param remnoise Remote background noise level + * @param rxerrors Receive errors + * @param fixed Count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} + +/** + * @brief Encode a radio_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack(system_id, component_id, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + +/** + * @brief Encode a radio_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + +/** + * @brief Send a radio_status message + * @param chan MAVLink channel to send the message + * + * @param rssi Local signal strength + * @param remrssi Remote signal strength + * @param txbuf Remaining free buffer space in percent. + * @param noise Background noise level + * @param remnoise Remote background noise level + * @param rxerrors Receive errors + * @param fixed Count of error corrected packets + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RADIO_STATUS UNPACKING + + +/** + * @brief Get field rssi from radio_status message + * + * @return Local signal strength + */ +static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field remrssi from radio_status message + * + * @return Remote signal strength + */ +static inline uint8_t mavlink_msg_radio_status_get_remrssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field txbuf from radio_status message + * + * @return Remaining free buffer space in percent. + */ +static inline uint8_t mavlink_msg_radio_status_get_txbuf(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field noise from radio_status message + * + * @return Background noise level + */ +static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field remnoise from radio_status message + * + * @return Remote background noise level + */ +static inline uint8_t mavlink_msg_radio_status_get_remnoise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field rxerrors from radio_status message + * + * @return Receive errors + */ +static inline uint16_t mavlink_msg_radio_status_get_rxerrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field fixed from radio_status message + * + * @return Count of error corrected packets + */ +static inline uint16_t mavlink_msg_radio_status_get_fixed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a radio_status message into a struct + * + * @param msg The message to decode + * @param radio_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_radio_status_decode(const mavlink_message_t* msg, mavlink_radio_status_t* radio_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + radio_status->rxerrors = mavlink_msg_radio_status_get_rxerrors(msg); + radio_status->fixed = mavlink_msg_radio_status_get_fixed(msg); + radio_status->rssi = mavlink_msg_radio_status_get_rssi(msg); + radio_status->remrssi = mavlink_msg_radio_status_get_remrssi(msg); + radio_status->txbuf = mavlink_msg_radio_status_get_txbuf(msg); + radio_status->noise = mavlink_msg_radio_status_get_noise(msg); + radio_status->remnoise = mavlink_msg_radio_status_get_remnoise(msg); +#else + memcpy(radio_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_raw_imu.h b/flightcode/mavlink/c_library/common/mavlink_msg_raw_imu.h new file mode 100644 index 0000000..a98e8ce --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_raw_imu.h @@ -0,0 +1,425 @@ +// MESSAGE RAW_IMU PACKING + +#define MAVLINK_MSG_ID_RAW_IMU 27 + +typedef struct __mavlink_raw_imu_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (raw) + int16_t yacc; ///< Y acceleration (raw) + int16_t zacc; ///< Z acceleration (raw) + int16_t xgyro; ///< Angular speed around X axis (raw) + int16_t ygyro; ///< Angular speed around Y axis (raw) + int16_t zgyro; ///< Angular speed around Z axis (raw) + int16_t xmag; ///< X Magnetic field (raw) + int16_t ymag; ///< Y Magnetic field (raw) + int16_t zmag; ///< Z Magnetic field (raw) +} mavlink_raw_imu_t; + +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_ID_27_LEN 26 + +#define MAVLINK_MSG_ID_RAW_IMU_CRC 144 +#define MAVLINK_MSG_ID_27_CRC 144 + + + +#define MAVLINK_MESSAGE_INFO_RAW_IMU { \ + "RAW_IMU", \ + 10, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a raw_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +} + +/** + * @brief Pack a raw_imu message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +} + +/** + * @brief Encode a raw_imu struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + +/** + * @brief Encode a raw_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + +/** + * @brief Send a raw_imu message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#else + mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RAW_IMU UNPACKING + + +/** + * @brief Get field time_usec from raw_imu message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_raw_imu_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from raw_imu message + * + * @return X acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field yacc from raw_imu message + * + * @return Y acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field zacc from raw_imu message + * + * @return Z acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field xgyro from raw_imu message + * + * @return Angular speed around X axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field ygyro from raw_imu message + * + * @return Angular speed around Y axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field zgyro from raw_imu message + * + * @return Angular speed around Z axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field xmag from raw_imu message + * + * @return X Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field ymag from raw_imu message + * + * @return Y Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field zmag from raw_imu message + * + * @return Z Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 24); +} + +/** + * @brief Decode a raw_imu message into a struct + * + * @param msg The message to decode + * @param raw_imu C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP + raw_imu->time_usec = mavlink_msg_raw_imu_get_time_usec(msg); + raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); + raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); + raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); + raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); + raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); + raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); + raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); + raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); + raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); +#else + memcpy(raw_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_raw_pressure.h b/flightcode/mavlink/c_library/common/mavlink_msg_raw_pressure.h new file mode 100644 index 0000000..6bd37fc --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_raw_pressure.h @@ -0,0 +1,305 @@ +// MESSAGE RAW_PRESSURE PACKING + +#define MAVLINK_MSG_ID_RAW_PRESSURE 28 + +typedef struct __mavlink_raw_pressure_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t press_abs; ///< Absolute pressure (raw) + int16_t press_diff1; ///< Differential pressure 1 (raw) + int16_t press_diff2; ///< Differential pressure 2 (raw) + int16_t temperature; ///< Raw Temperature measurement (raw) +} mavlink_raw_pressure_t; + +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_ID_28_LEN 16 + +#define MAVLINK_MSG_ID_RAW_PRESSURE_CRC 67 +#define MAVLINK_MSG_ID_28_CRC 67 + + + +#define MAVLINK_MESSAGE_INFO_RAW_PRESSURE { \ + "RAW_PRESSURE", \ + 5, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, time_usec) }, \ + { "press_abs", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \ + { "press_diff1", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \ + { "press_diff2", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \ + } \ +} + + +/** + * @brief Pack a raw_pressure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +} + +/** + * @brief Pack a raw_pressure message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,int16_t press_abs,int16_t press_diff1,int16_t press_diff2,int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +} + +/** + * @brief Encode a raw_pressure struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + +/** + * @brief Encode a raw_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + +/** + * @brief Send a raw_pressure message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#else + mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf; + packet->time_usec = time_usec; + packet->press_abs = press_abs; + packet->press_diff1 = press_diff1; + packet->press_diff2 = press_diff2; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RAW_PRESSURE UNPACKING + + +/** + * @brief Get field time_usec from raw_pressure message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_raw_pressure_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field press_abs from raw_pressure message + * + * @return Absolute pressure (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field press_diff1 from raw_pressure message + * + * @return Differential pressure 1 (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field press_diff2 from raw_pressure message + * + * @return Differential pressure 2 (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field temperature from raw_pressure message + * + * @return Raw Temperature measurement (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Decode a raw_pressure message into a struct + * + * @param msg The message to decode + * @param raw_pressure C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP + raw_pressure->time_usec = mavlink_msg_raw_pressure_get_time_usec(msg); + raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg); + raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg); + raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); + raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); +#else + memcpy(raw_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels.h b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels.h new file mode 100644 index 0000000..479af12 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels.h @@ -0,0 +1,689 @@ +// MESSAGE RC_CHANNELS PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS 65 + +typedef struct __mavlink_rc_channels_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan9_raw; ///< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan10_raw; ///< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan11_raw; ///< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan12_raw; ///< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan13_raw; ///< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan14_raw; ///< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan15_raw; ///< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan16_raw; ///< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan17_raw; ///< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan18_raw; ///< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint8_t chancount; ///< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. +} mavlink_rc_channels_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 +#define MAVLINK_MSG_ID_65_LEN 42 + +#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 +#define MAVLINK_MSG_ID_65_CRC 118 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ + "RC_CHANNELS", \ + 21, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ + { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Pack a rc_channels message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t chancount,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Encode a rc_channels struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Encode a rc_channels struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Send a rc_channels message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->chan13_raw = chan13_raw; + packet->chan14_raw = chan14_raw; + packet->chan15_raw = chan15_raw; + packet->chan16_raw = chan16_raw; + packet->chan17_raw = chan17_raw; + packet->chan18_raw = chan18_raw; + packet->chancount = chancount; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field chancount from rc_channels message + * + * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + */ +static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field chan1_raw from rc_channels message + * + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan2_raw from rc_channels message + * + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan3_raw from rc_channels message + * + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan4_raw from rc_channels message + * + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan5_raw from rc_channels message + * + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan6_raw from rc_channels message + * + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan7_raw from rc_channels message + * + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field chan8_raw from rc_channels message + * + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan9_raw from rc_channels message + * + * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan10_raw from rc_channels message + * + * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan11_raw from rc_channels message + * + * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan12_raw from rc_channels message + * + * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan13_raw from rc_channels message + * + * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan14_raw from rc_channels message + * + * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field chan15_raw from rc_channels message + * + * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field chan16_raw from rc_channels message + * + * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field chan17_raw from rc_channels message + * + * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field chan18_raw from rc_channels message + * + * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 38); +} + +/** + * @brief Get field rssi from rc_channels message + * + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Decode a rc_channels message into a struct + * + * @param msg The message to decode + * @param rc_channels C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); + rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); + rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); + rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); + rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); + rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); + rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); + rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); + rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); + rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); + rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); + rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); + rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); + rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); + rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); + rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); + rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); + rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); + rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); + rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); + rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); +#else + memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_override.h b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_override.h new file mode 100644 index 0000000..15d4c6f --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,425 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 + +typedef struct __mavlink_rc_channels_override_t +{ + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_rc_channels_override_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_ID_70_LEN 18 + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC 124 +#define MAVLINK_MSG_ID_70_CRC 124 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ + "RC_CHANNELS_OVERRIDE", \ + 10, \ + { { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +} + +/** + * @brief Pack a rc_channels_override message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +} + +/** + * @brief Encode a rc_channels_override struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Encode a rc_channels_override struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Send a rc_channels_override message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#else + mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + + +/** + * @brief Get field target_system from rc_channels_override message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field target_component from rc_channels_override message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field chan1_raw from rc_channels_override message + * + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field chan2_raw from rc_channels_override message + * + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field chan3_raw from rc_channels_override message + * + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan4_raw from rc_channels_override message + * + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan5_raw from rc_channels_override message + * + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan6_raw from rc_channels_override message + * + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan7_raw from rc_channels_override message + * + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan8_raw from rc_channels_override message + * + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Decode a rc_channels_override message into a struct + * + * @param msg The message to decode + * @param rc_channels_override C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg); + rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg); + rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg); + rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg); + rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg); + rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg); + rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg); + rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); + rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); + rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); +#else + memcpy(rc_channels_override, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_raw.h b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_raw.h new file mode 100644 index 0000000..d75a593 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_raw.h @@ -0,0 +1,449 @@ +// MESSAGE RC_CHANNELS_RAW PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 + +typedef struct __mavlink_rc_channels_raw_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. +} mavlink_rc_channels_raw_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 22 +#define MAVLINK_MSG_ID_35_LEN 22 + +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC 244 +#define MAVLINK_MSG_ID_35_CRC 244 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW { \ + "RC_CHANNELS_RAW", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, time_boot_ms) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \ + { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_raw_t, port) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_raw_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +} + +/** + * @brief Pack a rc_channels_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t port,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +} + +/** + * @brief Encode a rc_channels_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + +/** + * @brief Encode a rc_channels_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + +/** + * @brief Send a rc_channels_raw message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#else + mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS_RAW UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels_raw message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_rc_channels_raw_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field port from rc_channels_raw message + * + * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + */ +static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field chan1_raw from rc_channels_raw message + * + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan2_raw from rc_channels_raw message + * + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan3_raw from rc_channels_raw message + * + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan4_raw from rc_channels_raw message + * + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan5_raw from rc_channels_raw message + * + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan6_raw from rc_channels_raw message + * + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan7_raw from rc_channels_raw message + * + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field chan8_raw from rc_channels_raw message + * + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field rssi from rc_channels_raw message + * + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a rc_channels_raw message into a struct + * + * @param msg The message to decode + * @param rc_channels_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_raw->time_boot_ms = mavlink_msg_rc_channels_raw_get_time_boot_ms(msg); + rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); + rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); + rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); + rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); + rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); + rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); + rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); + rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); + rc_channels_raw->port = mavlink_msg_rc_channels_raw_get_port(msg); + rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); +#else + memcpy(rc_channels_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_scaled.h b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_scaled.h new file mode 100644 index 0000000..f400f98 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_rc_channels_scaled.h @@ -0,0 +1,449 @@ +// MESSAGE RC_CHANNELS_SCALED PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 34 + +typedef struct __mavlink_rc_channels_scaled_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. +} mavlink_rc_channels_scaled_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 22 +#define MAVLINK_MSG_ID_34_LEN 22 + +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC 237 +#define MAVLINK_MSG_ID_34_CRC 237 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED { \ + "RC_CHANNELS_SCALED", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, time_boot_ms) }, \ + { "chan1_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \ + { "chan2_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \ + { "chan3_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \ + { "chan4_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \ + { "chan5_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \ + { "chan6_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \ + { "chan7_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \ + { "chan8_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \ + { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_scaled_t, port) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_scaled message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +} + +/** + * @brief Pack a rc_channels_scaled message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t port,int16_t chan1_scaled,int16_t chan2_scaled,int16_t chan3_scaled,int16_t chan4_scaled,int16_t chan5_scaled,int16_t chan6_scaled,int16_t chan7_scaled,int16_t chan8_scaled,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +} + +/** + * @brief Encode a rc_channels_scaled struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + +/** + * @brief Encode a rc_channels_scaled struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + +/** + * @brief Send a rc_channels_scaled message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#else + mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_scaled = chan1_scaled; + packet->chan2_scaled = chan2_scaled; + packet->chan3_scaled = chan3_scaled; + packet->chan4_scaled = chan4_scaled; + packet->chan5_scaled = chan5_scaled; + packet->chan6_scaled = chan6_scaled; + packet->chan7_scaled = chan7_scaled; + packet->chan8_scaled = chan8_scaled; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS_SCALED UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels_scaled message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_rc_channels_scaled_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field port from rc_channels_scaled message + * + * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. + */ +static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field chan1_scaled from rc_channels_scaled message + * + * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field chan2_scaled from rc_channels_scaled message + * + * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field chan3_scaled from rc_channels_scaled message + * + * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field chan4_scaled from rc_channels_scaled message + * + * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field chan5_scaled from rc_channels_scaled message + * + * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field chan6_scaled from rc_channels_scaled message + * + * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field chan7_scaled from rc_channels_scaled message + * + * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field chan8_scaled from rc_channels_scaled message + * + * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field rssi from rc_channels_scaled message + * + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a rc_channels_scaled message into a struct + * + * @param msg The message to decode + * @param rc_channels_scaled C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_scaled->time_boot_ms = mavlink_msg_rc_channels_scaled_get_time_boot_ms(msg); + rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); + rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); + rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); + rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); + rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); + rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); + rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); + rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); + rc_channels_scaled->port = mavlink_msg_rc_channels_scaled_get_port(msg); + rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); +#else + memcpy(rc_channels_scaled, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_request_data_stream.h b/flightcode/mavlink/c_library/common/mavlink_msg_request_data_stream.h new file mode 100644 index 0000000..2ebcc54 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_request_data_stream.h @@ -0,0 +1,305 @@ +// MESSAGE REQUEST_DATA_STREAM PACKING + +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 + +typedef struct __mavlink_request_data_stream_t +{ + uint16_t req_message_rate; ///< The requested interval between two messages of this type + uint8_t target_system; ///< The target requested to send the message stream. + uint8_t target_component; ///< The target requested to send the message stream. + uint8_t req_stream_id; ///< The ID of the requested data stream + uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. +} mavlink_request_data_stream_t; + +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_ID_66_LEN 6 + +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC 148 +#define MAVLINK_MSG_ID_66_CRC 148 + + + +#define MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM { \ + "REQUEST_DATA_STREAM", \ + 5, \ + { { "req_message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_request_data_stream_t, target_component) }, \ + { "req_stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \ + { "start_stop", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \ + } \ +} + + +/** + * @brief Pack a request_data_stream message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested data stream + * @param req_message_rate The requested interval between two messages of this type + * @param start_stop 1 to start sending, 0 to stop sending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +} + +/** + * @brief Pack a request_data_stream message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested data stream + * @param req_message_rate The requested interval between two messages of this type + * @param start_stop 1 to start sending, 0 to stop sending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t req_stream_id,uint16_t req_message_rate,uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +} + +/** + * @brief Encode a request_data_stream struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + +/** + * @brief Encode a request_data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + +/** + * @brief Send a request_data_stream message + * @param chan MAVLink channel to send the message + * + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested data stream + * @param req_message_rate The requested interval between two messages of this type + * @param start_stop 1 to start sending, 0 to stop sending. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#else + mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf; + packet->req_message_rate = req_message_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->req_stream_id = req_stream_id; + packet->start_stop = start_stop; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE REQUEST_DATA_STREAM UNPACKING + + +/** + * @brief Get field target_system from request_data_stream message + * + * @return The target requested to send the message stream. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from request_data_stream message + * + * @return The target requested to send the message stream. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field req_stream_id from request_data_stream message + * + * @return The ID of the requested data stream + */ +static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field req_message_rate from request_data_stream message + * + * @return The requested interval between two messages of this type + */ +static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field start_stop from request_data_stream message + * + * @return 1 to start sending, 0 to stop sending. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Decode a request_data_stream message into a struct + * + * @param msg The message to decode + * @param request_data_stream C-struct to decode the message contents into + */ +static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) +{ +#if MAVLINK_NEED_BYTE_SWAP + request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); + request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); + request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); + request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); + request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); +#else + memcpy(request_data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_safety_allowed_area.h b/flightcode/mavlink/c_library/common/mavlink_msg_safety_allowed_area.h new file mode 100644 index 0000000..0176dfc --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_safety_allowed_area.h @@ -0,0 +1,353 @@ +// MESSAGE SAFETY_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 55 + +typedef struct __mavlink_safety_allowed_area_t +{ + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. +} mavlink_safety_allowed_area_t; + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_ID_55_LEN 25 + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC 3 +#define MAVLINK_MSG_ID_55_CRC 3 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA { \ + "SAFETY_ALLOWED_AREA", \ + 7, \ + { { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_allowed_area_t, p1x) }, \ + { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_allowed_area_t, p1y) }, \ + { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_allowed_area_t, p1z) }, \ + { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_allowed_area_t, p2x) }, \ + { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_allowed_area_t, p2y) }, \ + { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_allowed_area_t, p2z) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_allowed_area_t, frame) }, \ + } \ +} + + +/** + * @brief Pack a safety_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +} + +/** + * @brief Pack a safety_allowed_area message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +} + +/** + * @brief Encode a safety_allowed_area struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** + * @brief Encode a safety_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** + * @brief Send a safety_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SAFETY_ALLOWED_AREA UNPACKING + + +/** + * @brief Get field frame from safety_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field p1x from safety_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field p1y from safety_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field p1z from safety_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field p2x from safety_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field p2y from safety_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field p2z from safety_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a safety_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) +{ +#if MAVLINK_NEED_BYTE_SWAP + safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); + safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); + safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); + safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); + safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); + safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); + safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); +#else + memcpy(safety_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_safety_set_allowed_area.h b/flightcode/mavlink/c_library/common/mavlink_msg_safety_set_allowed_area.h new file mode 100644 index 0000000..dafbc26 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_safety_set_allowed_area.h @@ -0,0 +1,401 @@ +// MESSAGE SAFETY_SET_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 54 + +typedef struct __mavlink_safety_set_allowed_area_t +{ + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. +} mavlink_safety_set_allowed_area_t; + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_ID_54_LEN 27 + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC 15 +#define MAVLINK_MSG_ID_54_CRC 15 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA { \ + "SAFETY_SET_ALLOWED_AREA", \ + 9, \ + { { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \ + { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \ + { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \ + { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \ + { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \ + { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \ + } \ +} + + +/** + * @brief Pack a safety_set_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +} + +/** + * @brief Pack a safety_set_allowed_area message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +} + +/** + * @brief Encode a safety_set_allowed_area struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** + * @brief Encode a safety_set_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** + * @brief Send a safety_set_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING + + +/** + * @brief Get field target_system from safety_set_allowed_area message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field target_component from safety_set_allowed_area message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field frame from safety_set_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field p1x from safety_set_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field p1y from safety_set_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field p1z from safety_set_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field p2x from safety_set_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field p2y from safety_set_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field p2z from safety_set_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a safety_set_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_set_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ +#if MAVLINK_NEED_BYTE_SWAP + safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); + safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); + safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); + safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); + safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); + safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); + safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); + safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); + safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); +#else + memcpy(safety_set_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu.h b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu.h new file mode 100644 index 0000000..1648a56 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu.h @@ -0,0 +1,425 @@ +// MESSAGE SCALED_IMU PACKING + +#define MAVLINK_MSG_ID_SCALED_IMU 26 + +typedef struct __mavlink_scaled_imu_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) +} mavlink_scaled_imu_t; + +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 22 +#define MAVLINK_MSG_ID_26_LEN 22 + +#define MAVLINK_MSG_ID_SCALED_IMU_CRC 170 +#define MAVLINK_MSG_ID_26_CRC 170 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ + "SCALED_IMU", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a scaled_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +} + +/** + * @brief Pack a scaled_imu message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +} + +/** + * @brief Encode a scaled_imu struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + +/** + * @brief Encode a scaled_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + +/** + * @brief Send a scaled_imu message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#else + mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SCALED_IMU UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_imu message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_imu_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field xacc from scaled_imu message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field yacc from scaled_imu message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field zacc from scaled_imu message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field xgyro from scaled_imu message + * + * @return Angular speed around X axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field ygyro from scaled_imu message + * + * @return Angular speed around Y axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field zgyro from scaled_imu message + * + * @return Angular speed around Z axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field xmag from scaled_imu message + * + * @return X Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field ymag from scaled_imu message + * + * @return Y Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field zmag from scaled_imu message + * + * @return Z Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Decode a scaled_imu message into a struct + * + * @param msg The message to decode + * @param scaled_imu C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_imu->time_boot_ms = mavlink_msg_scaled_imu_get_time_boot_ms(msg); + scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); + scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); + scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); + scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); + scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); + scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); + scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); + scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); + scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); +#else + memcpy(scaled_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu2.h b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu2.h new file mode 100644 index 0000000..1dea121 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu2.h @@ -0,0 +1,425 @@ +// MESSAGE SCALED_IMU2 PACKING + +#define MAVLINK_MSG_ID_SCALED_IMU2 116 + +typedef struct __mavlink_scaled_imu2_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) +} mavlink_scaled_imu2_t; + +#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22 +#define MAVLINK_MSG_ID_116_LEN 22 + +#define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76 +#define MAVLINK_MSG_ID_116_CRC 76 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ + "SCALED_IMU2", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu2_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu2_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu2_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu2_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a scaled_imu2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} + +/** + * @brief Pack a scaled_imu2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} + +/** + * @brief Encode a scaled_imu2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param scaled_imu2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) +{ + return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); +} + +/** + * @brief Encode a scaled_imu2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) +{ + return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); +} + +/** + * @brief Send a scaled_imu2 message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SCALED_IMU2 UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_imu2 message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_imu2_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field xacc from scaled_imu2 message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field yacc from scaled_imu2 message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field zacc from scaled_imu2 message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field xgyro from scaled_imu2 message + * + * @return Angular speed around X axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field ygyro from scaled_imu2 message + * + * @return Angular speed around Y axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field zgyro from scaled_imu2 message + * + * @return Angular speed around Z axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field xmag from scaled_imu2 message + * + * @return X Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field ymag from scaled_imu2 message + * + * @return Y Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field zmag from scaled_imu2 message + * + * @return Z Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Decode a scaled_imu2 message into a struct + * + * @param msg The message to decode + * @param scaled_imu2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, mavlink_scaled_imu2_t* scaled_imu2) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_imu2->time_boot_ms = mavlink_msg_scaled_imu2_get_time_boot_ms(msg); + scaled_imu2->xacc = mavlink_msg_scaled_imu2_get_xacc(msg); + scaled_imu2->yacc = mavlink_msg_scaled_imu2_get_yacc(msg); + scaled_imu2->zacc = mavlink_msg_scaled_imu2_get_zacc(msg); + scaled_imu2->xgyro = mavlink_msg_scaled_imu2_get_xgyro(msg); + scaled_imu2->ygyro = mavlink_msg_scaled_imu2_get_ygyro(msg); + scaled_imu2->zgyro = mavlink_msg_scaled_imu2_get_zgyro(msg); + scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg); + scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg); + scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg); +#else + memcpy(scaled_imu2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu3.h b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu3.h new file mode 100644 index 0000000..73fc3a7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_imu3.h @@ -0,0 +1,425 @@ +// MESSAGE SCALED_IMU3 PACKING + +#define MAVLINK_MSG_ID_SCALED_IMU3 129 + +typedef struct __mavlink_scaled_imu3_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) +} mavlink_scaled_imu3_t; + +#define MAVLINK_MSG_ID_SCALED_IMU3_LEN 22 +#define MAVLINK_MSG_ID_129_LEN 22 + +#define MAVLINK_MSG_ID_SCALED_IMU3_CRC 46 +#define MAVLINK_MSG_ID_129_CRC 46 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU3 { \ + "SCALED_IMU3", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu3_t, time_boot_ms) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu3_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu3_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu3_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu3_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu3_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu3_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu3_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu3_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu3_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a scaled_imu3 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#else + mavlink_scaled_imu3_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +} + +/** + * @brief Pack a scaled_imu3 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#else + mavlink_scaled_imu3_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +} + +/** + * @brief Encode a scaled_imu3 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param scaled_imu3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) +{ + return mavlink_msg_scaled_imu3_pack(system_id, component_id, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); +} + +/** + * @brief Encode a scaled_imu3 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) +{ + return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); +} + +/** + * @brief Send a scaled_imu3 message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +#else + mavlink_scaled_imu3_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SCALED_IMU3_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +#else + mavlink_scaled_imu3_t *packet = (mavlink_scaled_imu3_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SCALED_IMU3 UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_imu3 message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_imu3_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field xacc from scaled_imu3 message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field yacc from scaled_imu3 message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field zacc from scaled_imu3 message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field xgyro from scaled_imu3 message + * + * @return Angular speed around X axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field ygyro from scaled_imu3 message + * + * @return Angular speed around Y axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field zgyro from scaled_imu3 message + * + * @return Angular speed around Z axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field xmag from scaled_imu3 message + * + * @return X Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field ymag from scaled_imu3 message + * + * @return Y Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field zmag from scaled_imu3 message + * + * @return Z Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu3_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Decode a scaled_imu3 message into a struct + * + * @param msg The message to decode + * @param scaled_imu3 C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_imu3_decode(const mavlink_message_t* msg, mavlink_scaled_imu3_t* scaled_imu3) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_imu3->time_boot_ms = mavlink_msg_scaled_imu3_get_time_boot_ms(msg); + scaled_imu3->xacc = mavlink_msg_scaled_imu3_get_xacc(msg); + scaled_imu3->yacc = mavlink_msg_scaled_imu3_get_yacc(msg); + scaled_imu3->zacc = mavlink_msg_scaled_imu3_get_zacc(msg); + scaled_imu3->xgyro = mavlink_msg_scaled_imu3_get_xgyro(msg); + scaled_imu3->ygyro = mavlink_msg_scaled_imu3_get_ygyro(msg); + scaled_imu3->zgyro = mavlink_msg_scaled_imu3_get_zgyro(msg); + scaled_imu3->xmag = mavlink_msg_scaled_imu3_get_xmag(msg); + scaled_imu3->ymag = mavlink_msg_scaled_imu3_get_ymag(msg); + scaled_imu3->zmag = mavlink_msg_scaled_imu3_get_zmag(msg); +#else + memcpy(scaled_imu3, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure.h b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure.h new file mode 100644 index 0000000..e0d6d87 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure.h @@ -0,0 +1,281 @@ +// MESSAGE SCALED_PRESSURE PACKING + +#define MAVLINK_MSG_ID_SCALED_PRESSURE 29 + +typedef struct __mavlink_scaled_pressure_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float press_abs; ///< Absolute pressure (hectopascal) + float press_diff; ///< Differential pressure 1 (hectopascal) + int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) +} mavlink_scaled_pressure_t; + +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 14 +#define MAVLINK_MSG_ID_29_LEN 14 + +#define MAVLINK_MSG_ID_SCALED_PRESSURE_CRC 115 +#define MAVLINK_MSG_ID_29_CRC 115 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ + "SCALED_PRESSURE", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ + { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ + { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + } \ +} + + +/** + * @brief Pack a scaled_pressure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +} + +/** + * @brief Pack a scaled_pressure message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +} + +/** + * @brief Encode a scaled_pressure struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + +/** + * @brief Encode a scaled_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + +/** + * @brief Send a scaled_pressure message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#else + mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->press_abs = press_abs; + packet->press_diff = press_diff; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SCALED_PRESSURE UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_pressure message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_pressure_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field press_abs from scaled_pressure message + * + * @return Absolute pressure (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field press_diff from scaled_pressure message + * + * @return Differential pressure 1 (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field temperature from scaled_pressure message + * + * @return Temperature measurement (0.01 degrees celsius) + */ +static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Decode a scaled_pressure message into a struct + * + * @param msg The message to decode + * @param scaled_pressure C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_pressure->time_boot_ms = mavlink_msg_scaled_pressure_get_time_boot_ms(msg); + scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); + scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); + scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); +#else + memcpy(scaled_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure2.h b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure2.h new file mode 100644 index 0000000..674f505 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_scaled_pressure2.h @@ -0,0 +1,281 @@ +// MESSAGE SCALED_PRESSURE2 PACKING + +#define MAVLINK_MSG_ID_SCALED_PRESSURE2 137 + +typedef struct __mavlink_scaled_pressure2_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + float press_abs; ///< Absolute pressure (hectopascal) + float press_diff; ///< Differential pressure 1 (hectopascal) + int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) +} mavlink_scaled_pressure2_t; + +#define MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN 14 +#define MAVLINK_MSG_ID_137_LEN 14 + +#define MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC 195 +#define MAVLINK_MSG_ID_137_CRC 195 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2 { \ + "SCALED_PRESSURE2", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure2_t, time_boot_ms) }, \ + { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure2_t, press_abs) }, \ + { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure2_t, press_diff) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure2_t, temperature) }, \ + } \ +} + + +/** + * @brief Pack a scaled_pressure2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#else + mavlink_scaled_pressure2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +} + +/** + * @brief Pack a scaled_pressure2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#else + mavlink_scaled_pressure2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +} + +/** + * @brief Encode a scaled_pressure2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) +{ + return mavlink_msg_scaled_pressure2_pack(system_id, component_id, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); +} + +/** + * @brief Encode a scaled_pressure2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) +{ + return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); +} + +/** + * @brief Send a scaled_pressure2 message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +#else + mavlink_scaled_pressure2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +#else + mavlink_scaled_pressure2_t *packet = (mavlink_scaled_pressure2_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->press_abs = press_abs; + packet->press_diff = press_diff; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SCALED_PRESSURE2 UNPACKING + + +/** + * @brief Get field time_boot_ms from scaled_pressure2 message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_scaled_pressure2_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field press_abs from scaled_pressure2 message + * + * @return Absolute pressure (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure2_get_press_abs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field press_diff from scaled_pressure2 message + * + * @return Differential pressure 1 (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure2_get_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field temperature from scaled_pressure2 message + * + * @return Temperature measurement (0.01 degrees celsius) + */ +static inline int16_t mavlink_msg_scaled_pressure2_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Decode a scaled_pressure2 message into a struct + * + * @param msg The message to decode + * @param scaled_pressure2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_pressure2_decode(const mavlink_message_t* msg, mavlink_scaled_pressure2_t* scaled_pressure2) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_pressure2->time_boot_ms = mavlink_msg_scaled_pressure2_get_time_boot_ms(msg); + scaled_pressure2->press_abs = mavlink_msg_scaled_pressure2_get_press_abs(msg); + scaled_pressure2->press_diff = mavlink_msg_scaled_pressure2_get_press_diff(msg); + scaled_pressure2->temperature = mavlink_msg_scaled_pressure2_get_temperature(msg); +#else + memcpy(scaled_pressure2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_serial_control.h b/flightcode/mavlink/c_library/common/mavlink_msg_serial_control.h new file mode 100644 index 0000000..cbf72ba --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_serial_control.h @@ -0,0 +1,321 @@ +// MESSAGE SERIAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_SERIAL_CONTROL 126 + +typedef struct __mavlink_serial_control_t +{ + uint32_t baudrate; ///< Baudrate of transfer. Zero means no change. + uint16_t timeout; ///< Timeout for reply data in milliseconds + uint8_t device; ///< See SERIAL_CONTROL_DEV enum + uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum + uint8_t count; ///< how many bytes in this transfer + uint8_t data[70]; ///< serial data +} mavlink_serial_control_t; + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 +#define MAVLINK_MSG_ID_126_LEN 79 + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 +#define MAVLINK_MSG_ID_126_CRC 220 + +#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70 + +#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ + "SERIAL_CONTROL", \ + 6, \ + { { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ + { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ + { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + } \ +} + + +/** + * @brief Pack a serial_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a serial_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a serial_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Encode a serial_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Send a serial_control message + * @param chan MAVLink channel to send the message + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf; + packet->baudrate = baudrate; + packet->timeout = timeout; + packet->device = device; + packet->flags = flags; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SERIAL_CONTROL UNPACKING + + +/** + * @brief Get field device from serial_control message + * + * @return See SERIAL_CONTROL_DEV enum + */ +static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field flags from serial_control message + * + * @return See SERIAL_CONTROL_FLAG enum + */ +static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field timeout from serial_control message + * + * @return Timeout for reply data in milliseconds + */ +static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field baudrate from serial_control message + * + * @return Baudrate of transfer. Zero means no change. + */ +static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from serial_control message + * + * @return how many bytes in this transfer + */ +static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field data from serial_control message + * + * @return serial data + */ +static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); +} + +/** + * @brief Decode a serial_control message into a struct + * + * @param msg The message to decode + * @param serial_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg); + serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg); + serial_control->device = mavlink_msg_serial_control_get_device(msg); + serial_control->flags = mavlink_msg_serial_control_get_flags(msg); + serial_control->count = mavlink_msg_serial_control_get_count(msg); + mavlink_msg_serial_control_get_data(msg, serial_control->data); +#else + memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_servo_output_raw.h b/flightcode/mavlink/c_library/common/mavlink_msg_servo_output_raw.h new file mode 100644 index 0000000..e840886 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_servo_output_raw.h @@ -0,0 +1,425 @@ +// MESSAGE SERVO_OUTPUT_RAW PACKING + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36 + +typedef struct __mavlink_servo_output_raw_t +{ + uint32_t time_usec; ///< Timestamp (microseconds since system boot) + uint16_t servo1_raw; ///< Servo output 1 value, in microseconds + uint16_t servo2_raw; ///< Servo output 2 value, in microseconds + uint16_t servo3_raw; ///< Servo output 3 value, in microseconds + uint16_t servo4_raw; ///< Servo output 4 value, in microseconds + uint16_t servo5_raw; ///< Servo output 5 value, in microseconds + uint16_t servo6_raw; ///< Servo output 6 value, in microseconds + uint16_t servo7_raw; ///< Servo output 7 value, in microseconds + uint16_t servo8_raw; ///< Servo output 8 value, in microseconds + uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. +} mavlink_servo_output_raw_t; + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21 +#define MAVLINK_MSG_ID_36_LEN 21 + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222 +#define MAVLINK_MSG_ID_36_CRC 222 + + + +#define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ + "SERVO_OUTPUT_RAW", \ + 10, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ + { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ + { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \ + { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \ + { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \ + { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \ + { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ + { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ + { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ + } \ +} + + +/** + * @brief Pack a servo_output_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +} + +/** + * @brief Pack a servo_output_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +} + +/** + * @brief Encode a servo_output_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** + * @brief Encode a servo_output_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** + * @brief Send a servo_output_raw message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since system boot) + * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#else + mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->servo1_raw = servo1_raw; + packet->servo2_raw = servo2_raw; + packet->servo3_raw = servo3_raw; + packet->servo4_raw = servo4_raw; + packet->servo5_raw = servo5_raw; + packet->servo6_raw = servo6_raw; + packet->servo7_raw = servo7_raw; + packet->servo8_raw = servo8_raw; + packet->port = port; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SERVO_OUTPUT_RAW UNPACKING + + +/** + * @brief Get field time_usec from servo_output_raw message + * + * @return Timestamp (microseconds since system boot) + */ +static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field port from servo_output_raw message + * + * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + */ +static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field servo1_raw from servo_output_raw message + * + * @return Servo output 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field servo2_raw from servo_output_raw message + * + * @return Servo output 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field servo3_raw from servo_output_raw message + * + * @return Servo output 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field servo4_raw from servo_output_raw message + * + * @return Servo output 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field servo5_raw from servo_output_raw message + * + * @return Servo output 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field servo6_raw from servo_output_raw message + * + * @return Servo output 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field servo7_raw from servo_output_raw message + * + * @return Servo output 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field servo8_raw from servo_output_raw message + * + * @return Servo output 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Decode a servo_output_raw message into a struct + * + * @param msg The message to decode + * @param servo_output_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + servo_output_raw->time_usec = mavlink_msg_servo_output_raw_get_time_usec(msg); + servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); + servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); + servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); + servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); + servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); + servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); + servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); + servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); + servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg); +#else + memcpy(servo_output_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_actuator_control_target.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_actuator_control_target.h new file mode 100644 index 0000000..50d68a7 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_actuator_control_target.h @@ -0,0 +1,297 @@ +// MESSAGE SET_ACTUATOR_CONTROL_TARGET PACKING + +#define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET 139 + +typedef struct __mavlink_set_actuator_control_target_t +{ + uint64_t time_usec; ///< Timestamp (micros since boot or Unix epoch) + float controls[8]; ///< Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + uint8_t group_mlx; ///< Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_set_actuator_control_target_t; + +#define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN 43 +#define MAVLINK_MSG_ID_139_LEN 43 + +#define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC 168 +#define MAVLINK_MSG_ID_139_CRC 168 + +#define MAVLINK_MSG_SET_ACTUATOR_CONTROL_TARGET_FIELD_CONTROLS_LEN 8 + +#define MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET { \ + "SET_ACTUATOR_CONTROL_TARGET", \ + 5, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_set_actuator_control_target_t, time_usec) }, \ + { "controls", NULL, MAVLINK_TYPE_FLOAT, 8, 8, offsetof(mavlink_set_actuator_control_target_t, controls) }, \ + { "group_mlx", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_set_actuator_control_target_t, group_mlx) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_set_actuator_control_target_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_set_actuator_control_target_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a set_actuator_control_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param target_system System ID + * @param target_component Component ID + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t group_mlx, uint8_t target_system, uint8_t target_component, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_uint8_t(buf, 41, target_system); + _mav_put_uint8_t(buf, 42, target_component); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_set_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + +/** + * @brief Pack a set_actuator_control_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param target_system System ID + * @param target_component Component ID + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t group_mlx,uint8_t target_system,uint8_t target_component,const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_uint8_t(buf, 41, target_system); + _mav_put_uint8_t(buf, 42, target_component); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_set_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + +/** + * @brief Encode a set_actuator_control_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_actuator_control_target_t* set_actuator_control_target) +{ + return mavlink_msg_set_actuator_control_target_pack(system_id, component_id, msg, set_actuator_control_target->time_usec, set_actuator_control_target->group_mlx, set_actuator_control_target->target_system, set_actuator_control_target->target_component, set_actuator_control_target->controls); +} + +/** + * @brief Encode a set_actuator_control_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_actuator_control_target_t* set_actuator_control_target) +{ + return mavlink_msg_set_actuator_control_target_pack_chan(system_id, component_id, chan, msg, set_actuator_control_target->time_usec, set_actuator_control_target->group_mlx, set_actuator_control_target->target_system, set_actuator_control_target->target_component, set_actuator_control_target->controls); +} + +/** + * @brief Send a set_actuator_control_target message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (micros since boot or Unix epoch) + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param target_system System ID + * @param target_component Component ID + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_actuator_control_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t group_mlx, uint8_t target_system, uint8_t target_component, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_uint8_t(buf, 41, target_system); + _mav_put_uint8_t(buf, 42, target_component); + _mav_put_float_array(buf, 8, controls, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#else + mavlink_set_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_actuator_control_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t group_mlx, uint8_t target_system, uint8_t target_component, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_uint8_t(buf, 41, target_system); + _mav_put_uint8_t(buf, 42, target_component); + _mav_put_float_array(buf, 8, controls, 8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#else + mavlink_set_actuator_control_target_t *packet = (mavlink_set_actuator_control_target_t *)msgbuf; + packet->time_usec = time_usec; + packet->group_mlx = group_mlx; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->controls, controls, sizeof(float)*8); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_ACTUATOR_CONTROL_TARGET UNPACKING + + +/** + * @brief Get field time_usec from set_actuator_control_target message + * + * @return Timestamp (micros since boot or Unix epoch) + */ +static inline uint64_t mavlink_msg_set_actuator_control_target_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field group_mlx from set_actuator_control_target message + * + * @return Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + */ +static inline uint8_t mavlink_msg_set_actuator_control_target_get_group_mlx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field target_system from set_actuator_control_target message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_actuator_control_target_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field target_component from set_actuator_control_target message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_actuator_control_target_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field controls from set_actuator_control_target message + * + * @return Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_get_controls(const mavlink_message_t* msg, float *controls) +{ + return _MAV_RETURN_float_array(msg, controls, 8, 8); +} + +/** + * @brief Decode a set_actuator_control_target message into a struct + * + * @param msg The message to decode + * @param set_actuator_control_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_actuator_control_target_decode(const mavlink_message_t* msg, mavlink_set_actuator_control_target_t* set_actuator_control_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_actuator_control_target->time_usec = mavlink_msg_set_actuator_control_target_get_time_usec(msg); + mavlink_msg_set_actuator_control_target_get_controls(msg, set_actuator_control_target->controls); + set_actuator_control_target->group_mlx = mavlink_msg_set_actuator_control_target_get_group_mlx(msg); + set_actuator_control_target->target_system = mavlink_msg_set_actuator_control_target_get_target_system(msg); + set_actuator_control_target->target_component = mavlink_msg_set_actuator_control_target_get_target_component(msg); +#else + memcpy(set_actuator_control_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_attitude_target.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_attitude_target.h new file mode 100644 index 0000000..19ccc14 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_attitude_target.h @@ -0,0 +1,393 @@ +// MESSAGE SET_ATTITUDE_TARGET PACKING + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET 82 + +typedef struct __mavlink_set_attitude_target_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float q[4]; ///< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + float body_roll_rate; ///< Body roll rate in radians per second + float body_pitch_rate; ///< Body roll rate in radians per second + float body_yaw_rate; ///< Body roll rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t type_mask; ///< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude +} mavlink_set_attitude_target_t; + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 39 +#define MAVLINK_MSG_ID_82_LEN 39 + +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC 49 +#define MAVLINK_MSG_ID_82_CRC 49 + +#define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \ + "SET_ATTITUDE_TARGET", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_set_attitude_target_t, q) }, \ + { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_attitude_target_t, body_roll_rate) }, \ + { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \ + { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_set_attitude_target_t, type_mask) }, \ + } \ +} + + +/** + * @brief Pack a set_attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Pack a set_attitude_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} + +/** + * @brief Encode a set_attitude_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) +{ + return mavlink_msg_set_attitude_target_pack(system_id, component_id, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); +} + +/** + * @brief Encode a set_attitude_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) +{ + return mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, chan, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); +} + +/** + * @brief Send a set_attitude_target message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#else + mavlink_set_attitude_target_t *packet = (mavlink_set_attitude_target_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->body_roll_rate = body_roll_rate; + packet->body_pitch_rate = body_pitch_rate; + packet->body_yaw_rate = body_yaw_rate; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type_mask = type_mask; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_ATTITUDE_TARGET UNPACKING + + +/** + * @brief Get field time_boot_ms from set_attitude_target message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_set_attitude_target_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_attitude_target message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field target_component from set_attitude_target message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Get field type_mask from set_attitude_target message + * + * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + */ +static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field q from set_attitude_target message + * + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_set_attitude_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field body_roll_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_roll_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field body_pitch_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field body_yaw_rate from set_attitude_target message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_set_attitude_target_get_body_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field thrust from set_attitude_target message + * + * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +static inline float mavlink_msg_set_attitude_target_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a set_attitude_target message into a struct + * + * @param msg The message to decode + * @param set_attitude_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_attitude_target_decode(const mavlink_message_t* msg, mavlink_set_attitude_target_t* set_attitude_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_attitude_target->time_boot_ms = mavlink_msg_set_attitude_target_get_time_boot_ms(msg); + mavlink_msg_set_attitude_target_get_q(msg, set_attitude_target->q); + set_attitude_target->body_roll_rate = mavlink_msg_set_attitude_target_get_body_roll_rate(msg); + set_attitude_target->body_pitch_rate = mavlink_msg_set_attitude_target_get_body_pitch_rate(msg); + set_attitude_target->body_yaw_rate = mavlink_msg_set_attitude_target_get_body_yaw_rate(msg); + set_attitude_target->thrust = mavlink_msg_set_attitude_target_get_thrust(msg); + set_attitude_target->target_system = mavlink_msg_set_attitude_target_get_target_system(msg); + set_attitude_target->target_component = mavlink_msg_set_attitude_target_get_target_component(msg); + set_attitude_target->type_mask = mavlink_msg_set_attitude_target_get_type_mask(msg); +#else + memcpy(set_attitude_target, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_gps_global_origin.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_gps_global_origin.h new file mode 100644 index 0000000..f5d918f --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_gps_global_origin.h @@ -0,0 +1,281 @@ +// MESSAGE SET_GPS_GLOBAL_ORIGIN PACKING + +#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48 + +typedef struct __mavlink_set_gps_global_origin_t +{ + int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 + int32_t longitude; ///< Longitude (WGS84, in degrees * 1E7 + int32_t altitude; ///< Altitude (AMSL), in meters * 1000 (positive for up) + uint8_t target_system; ///< System ID +} mavlink_set_gps_global_origin_t; + +#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 13 +#define MAVLINK_MSG_ID_48_LEN 13 + +#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC 41 +#define MAVLINK_MSG_ID_48_CRC 41 + + + +#define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ + "SET_GPS_GLOBAL_ORIGIN", \ + 4, \ + { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ + { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ + } \ +} + + +/** + * @brief Pack a set_gps_global_origin message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84, in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + +/** + * @brief Pack a set_gps_global_origin message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84, in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + +/** + * @brief Encode a set_gps_global_origin struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); +} + +/** + * @brief Encode a set_gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); +} + +/** + * @brief Send a set_gps_global_origin message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param latitude Latitude (WGS84), in degrees * 1E7 + * @param longitude Longitude (WGS84, in degrees * 1E7 + * @param altitude Altitude (AMSL), in meters * 1000 (positive for up) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING + + +/** + * @brief Get field target_system from set_gps_global_origin message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field latitude from set_gps_global_origin message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field longitude from set_gps_global_origin message + * + * @return Longitude (WGS84, in degrees * 1E7 + */ +static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field altitude from set_gps_global_origin message + * + * @return Altitude (AMSL), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Decode a set_gps_global_origin message into a struct + * + * @param msg The message to decode + * @param set_gps_global_origin C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_gps_global_origin->latitude = mavlink_msg_set_gps_global_origin_get_latitude(msg); + set_gps_global_origin->longitude = mavlink_msg_set_gps_global_origin_get_longitude(msg); + set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg); + set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg); +#else + memcpy(set_gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_mode.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_mode.h new file mode 100644 index 0000000..4b60a41 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_mode.h @@ -0,0 +1,257 @@ +// MESSAGE SET_MODE PACKING + +#define MAVLINK_MSG_ID_SET_MODE 11 + +typedef struct __mavlink_set_mode_t +{ + uint32_t custom_mode; ///< The new autopilot-specific mode. This field can be ignored by an autopilot. + uint8_t target_system; ///< The system setting the mode + uint8_t base_mode; ///< The new base mode +} mavlink_set_mode_t; + +#define MAVLINK_MSG_ID_SET_MODE_LEN 6 +#define MAVLINK_MSG_ID_11_LEN 6 + +#define MAVLINK_MSG_ID_SET_MODE_CRC 89 +#define MAVLINK_MSG_ID_11_CRC 89 + + + +#define MAVLINK_MESSAGE_INFO_SET_MODE { \ + "SET_MODE", \ + 3, \ + { { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_mode_t, custom_mode) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_set_mode_t, target_system) }, \ + { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_set_mode_t, base_mode) }, \ + } \ +} + + +/** + * @brief Pack a set_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system The system setting the mode + * @param base_mode The new base mode + * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +} + +/** + * @brief Pack a set_mode message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system The system setting the mode + * @param base_mode The new base mode + * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t base_mode,uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +} + +/** + * @brief Encode a set_mode struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + +/** + * @brief Encode a set_mode struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + +/** + * @brief Send a set_mode message + * @param chan MAVLink channel to send the message + * + * @param target_system The system setting the mode + * @param base_mode The new base mode + * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#else + mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->target_system = target_system; + packet->base_mode = base_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_MODE UNPACKING + + +/** + * @brief Get field target_system from set_mode message + * + * @return The system setting the mode + */ +static inline uint8_t mavlink_msg_set_mode_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field base_mode from set_mode message + * + * @return The new base mode + */ +static inline uint8_t mavlink_msg_set_mode_get_base_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field custom_mode from set_mode message + * + * @return The new autopilot-specific mode. This field can be ignored by an autopilot. + */ +static inline uint32_t mavlink_msg_set_mode_get_custom_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Decode a set_mode message into a struct + * + * @param msg The message to decode + * @param set_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_mode->custom_mode = mavlink_msg_set_mode_get_custom_mode(msg); + set_mode->target_system = mavlink_msg_set_mode_get_target_system(msg); + set_mode->base_mode = mavlink_msg_set_mode_get_base_mode(msg); +#else + memcpy(set_mode, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_global_int.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_global_int.h new file mode 100644 index 0000000..e528d73 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_global_int.h @@ -0,0 +1,569 @@ +// MESSAGE SET_POSITION_TARGET_GLOBAL_INT PACKING + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT 86 + +typedef struct __mavlink_set_position_target_global_int_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + int32_t lat_int; ///< X Position in WGS84 frame in 1e7 * meters + int32_t lon_int; ///< Y Position in WGS84 frame in 1e7 * meters + float alt; ///< Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +} mavlink_set_position_target_global_int_t; + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN 53 +#define MAVLINK_MSG_ID_86_LEN 53 + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC 5 +#define MAVLINK_MSG_ID_86_CRC 5 + + + +#define MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT { \ + "SET_POSITION_TARGET_GLOBAL_INT", \ + 16, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_position_target_global_int_t, time_boot_ms) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_position_target_global_int_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_position_target_global_int_t, lon_int) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_target_global_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_set_position_target_global_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_position_target_global_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_position_target_global_int_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_position_target_global_int_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_position_target_global_int_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_set_position_target_global_int_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_position_target_global_int_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_position_target_global_int_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_set_position_target_global_int_t, type_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_set_position_target_global_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_set_position_target_global_int_t, target_component) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_position_target_global_int_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a set_position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Pack a set_position_target_global_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t type_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + +/** + * @brief Encode a set_position_target_global_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ + return mavlink_msg_set_position_target_global_int_pack(system_id, component_id, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); +} + +/** + * @brief Encode a set_position_target_global_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ + return mavlink_msg_set_position_target_global_int_pack_chan(system_id, component_id, chan, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); +} + +/** + * @brief Send a set_position_target_global_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_position_target_global_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_target_global_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#else + mavlink_set_position_target_global_int_t *packet = (mavlink_set_position_target_global_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_int = lat_int; + packet->lon_int = lon_int; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_POSITION_TARGET_GLOBAL_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from set_position_target_global_int message + * + * @return Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + */ +static inline uint32_t mavlink_msg_set_position_target_global_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_position_target_global_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from set_position_target_global_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field coordinate_frame from set_position_target_global_int message + * + * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + */ +static inline uint8_t mavlink_msg_set_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field type_mask from set_position_target_global_int message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field lat_int from set_position_target_global_int message + * + * @return X Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_set_position_target_global_int_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_int from set_position_target_global_int message + * + * @return Y Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_set_position_target_global_int_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from set_position_target_global_int message + * + * @return Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + */ +static inline float mavlink_msg_set_position_target_global_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from set_position_target_global_int message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from set_position_target_global_int message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from set_position_target_global_int message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_global_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from set_position_target_global_int message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from set_position_target_global_int message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from set_position_target_global_int message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_global_int_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from set_position_target_global_int message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_set_position_target_global_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from set_position_target_global_int message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_set_position_target_global_int_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a set_position_target_global_int message into a struct + * + * @param msg The message to decode + * @param set_position_target_global_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_position_target_global_int_decode(const mavlink_message_t* msg, mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_position_target_global_int->time_boot_ms = mavlink_msg_set_position_target_global_int_get_time_boot_ms(msg); + set_position_target_global_int->lat_int = mavlink_msg_set_position_target_global_int_get_lat_int(msg); + set_position_target_global_int->lon_int = mavlink_msg_set_position_target_global_int_get_lon_int(msg); + set_position_target_global_int->alt = mavlink_msg_set_position_target_global_int_get_alt(msg); + set_position_target_global_int->vx = mavlink_msg_set_position_target_global_int_get_vx(msg); + set_position_target_global_int->vy = mavlink_msg_set_position_target_global_int_get_vy(msg); + set_position_target_global_int->vz = mavlink_msg_set_position_target_global_int_get_vz(msg); + set_position_target_global_int->afx = mavlink_msg_set_position_target_global_int_get_afx(msg); + set_position_target_global_int->afy = mavlink_msg_set_position_target_global_int_get_afy(msg); + set_position_target_global_int->afz = mavlink_msg_set_position_target_global_int_get_afz(msg); + set_position_target_global_int->yaw = mavlink_msg_set_position_target_global_int_get_yaw(msg); + set_position_target_global_int->yaw_rate = mavlink_msg_set_position_target_global_int_get_yaw_rate(msg); + set_position_target_global_int->type_mask = mavlink_msg_set_position_target_global_int_get_type_mask(msg); + set_position_target_global_int->target_system = mavlink_msg_set_position_target_global_int_get_target_system(msg); + set_position_target_global_int->target_component = mavlink_msg_set_position_target_global_int_get_target_component(msg); + set_position_target_global_int->coordinate_frame = mavlink_msg_set_position_target_global_int_get_coordinate_frame(msg); +#else + memcpy(set_position_target_global_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_local_ned.h b/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_local_ned.h new file mode 100644 index 0000000..8fcc3cb --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_set_position_target_local_ned.h @@ -0,0 +1,569 @@ +// MESSAGE SET_POSITION_TARGET_LOCAL_NED PACKING + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED 84 + +typedef struct __mavlink_set_position_target_local_ned_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float x; ///< X Position in NED frame in meters + float y; ///< Y Position in NED frame in meters + float z; ///< Z Position in NED frame in meters (note, altitude is negative in NED) + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float yaw; ///< yaw setpoint in rad + float yaw_rate; ///< yaw rate setpoint in rad/s + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 +} mavlink_set_position_target_local_ned_t; + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN 53 +#define MAVLINK_MSG_ID_84_LEN 53 + +#define MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC 143 +#define MAVLINK_MSG_ID_84_CRC 143 + + + +#define MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED { \ + "SET_POSITION_TARGET_LOCAL_NED", \ + 16, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_position_target_local_ned_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_position_target_local_ned_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_position_target_local_ned_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_target_local_ned_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_set_position_target_local_ned_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_position_target_local_ned_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_position_target_local_ned_t, vz) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_position_target_local_ned_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_position_target_local_ned_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_set_position_target_local_ned_t, afz) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_position_target_local_ned_t, yaw) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_position_target_local_ned_t, yaw_rate) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_set_position_target_local_ned_t, type_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_set_position_target_local_ned_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_set_position_target_local_ned_t, target_component) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_position_target_local_ned_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a set_position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Pack a set_position_target_local_ned message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t type_mask,float x,float y,float z,float vx,float vy,float vz,float afx,float afy,float afz,float yaw,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + +/** + * @brief Encode a set_position_target_local_ned struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ + return mavlink_msg_set_position_target_local_ned_pack(system_id, component_id, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); +} + +/** + * @brief Encode a set_position_target_local_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ + return mavlink_msg_set_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); +} + +/** + * @brief Send a set_position_target_local_ned message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw yaw setpoint in rad + * @param yaw_rate yaw rate setpoint in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_position_target_local_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_target_local_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#else + mavlink_set_position_target_local_ned_t *packet = (mavlink_set_position_target_local_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->yaw = yaw; + packet->yaw_rate = yaw_rate; + packet->type_mask = type_mask; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SET_POSITION_TARGET_LOCAL_NED UNPACKING + + +/** + * @brief Get field time_boot_ms from set_position_target_local_ned message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_set_position_target_local_ned_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from set_position_target_local_ned message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from set_position_target_local_ned message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field coordinate_frame from set_position_target_local_ned message + * + * @return Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + */ +static inline uint8_t mavlink_msg_set_position_target_local_ned_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field type_mask from set_position_target_local_ned message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_get_type_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field x from set_position_target_local_ned message + * + * @return X Position in NED frame in meters + */ +static inline float mavlink_msg_set_position_target_local_ned_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from set_position_target_local_ned message + * + * @return Y Position in NED frame in meters + */ +static inline float mavlink_msg_set_position_target_local_ned_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from set_position_target_local_ned message + * + * @return Z Position in NED frame in meters (note, altitude is negative in NED) + */ +static inline float mavlink_msg_set_position_target_local_ned_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from set_position_target_local_ned message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from set_position_target_local_ned message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from set_position_target_local_ned message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field afx from set_position_target_local_ned message + * + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field afy from set_position_target_local_ned message + * + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field afz from set_position_target_local_ned message + * + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + */ +static inline float mavlink_msg_set_position_target_local_ned_get_afz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw from set_position_target_local_ned message + * + * @return yaw setpoint in rad + */ +static inline float mavlink_msg_set_position_target_local_ned_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field yaw_rate from set_position_target_local_ned message + * + * @return yaw rate setpoint in rad/s + */ +static inline float mavlink_msg_set_position_target_local_ned_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a set_position_target_local_ned message into a struct + * + * @param msg The message to decode + * @param set_position_target_local_ned C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_position_target_local_ned_decode(const mavlink_message_t* msg, mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_position_target_local_ned->time_boot_ms = mavlink_msg_set_position_target_local_ned_get_time_boot_ms(msg); + set_position_target_local_ned->x = mavlink_msg_set_position_target_local_ned_get_x(msg); + set_position_target_local_ned->y = mavlink_msg_set_position_target_local_ned_get_y(msg); + set_position_target_local_ned->z = mavlink_msg_set_position_target_local_ned_get_z(msg); + set_position_target_local_ned->vx = mavlink_msg_set_position_target_local_ned_get_vx(msg); + set_position_target_local_ned->vy = mavlink_msg_set_position_target_local_ned_get_vy(msg); + set_position_target_local_ned->vz = mavlink_msg_set_position_target_local_ned_get_vz(msg); + set_position_target_local_ned->afx = mavlink_msg_set_position_target_local_ned_get_afx(msg); + set_position_target_local_ned->afy = mavlink_msg_set_position_target_local_ned_get_afy(msg); + set_position_target_local_ned->afz = mavlink_msg_set_position_target_local_ned_get_afz(msg); + set_position_target_local_ned->yaw = mavlink_msg_set_position_target_local_ned_get_yaw(msg); + set_position_target_local_ned->yaw_rate = mavlink_msg_set_position_target_local_ned_get_yaw_rate(msg); + set_position_target_local_ned->type_mask = mavlink_msg_set_position_target_local_ned_get_type_mask(msg); + set_position_target_local_ned->target_system = mavlink_msg_set_position_target_local_ned_get_target_system(msg); + set_position_target_local_ned->target_component = mavlink_msg_set_position_target_local_ned_get_target_component(msg); + set_position_target_local_ned->coordinate_frame = mavlink_msg_set_position_target_local_ned_get_coordinate_frame(msg); +#else + memcpy(set_position_target_local_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_sim_state.h b/flightcode/mavlink/c_library/common/mavlink_msg_sim_state.h new file mode 100644 index 0000000..c47fc8c --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_sim_state.h @@ -0,0 +1,689 @@ +// MESSAGE SIM_STATE PACKING + +#define MAVLINK_MSG_ID_SIM_STATE 108 + +typedef struct __mavlink_sim_state_t +{ + float q1; ///< True attitude quaternion component 1, w (1 in null-rotation) + float q2; ///< True attitude quaternion component 2, x (0 in null-rotation) + float q3; ///< True attitude quaternion component 3, y (0 in null-rotation) + float q4; ///< True attitude quaternion component 4, z (0 in null-rotation) + float roll; ///< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + float pitch; ///< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + float yaw; ///< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + float xacc; ///< X acceleration m/s/s + float yacc; ///< Y acceleration m/s/s + float zacc; ///< Z acceleration m/s/s + float xgyro; ///< Angular speed around X axis rad/s + float ygyro; ///< Angular speed around Y axis rad/s + float zgyro; ///< Angular speed around Z axis rad/s + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float std_dev_horz; ///< Horizontal position standard deviation + float std_dev_vert; ///< Vertical position standard deviation + float vn; ///< True velocity in m/s in NORTH direction in earth-fixed NED frame + float ve; ///< True velocity in m/s in EAST direction in earth-fixed NED frame + float vd; ///< True velocity in m/s in DOWN direction in earth-fixed NED frame +} mavlink_sim_state_t; + +#define MAVLINK_MSG_ID_SIM_STATE_LEN 84 +#define MAVLINK_MSG_ID_108_LEN 84 + +#define MAVLINK_MSG_ID_SIM_STATE_CRC 32 +#define MAVLINK_MSG_ID_108_CRC 32 + + + +#define MAVLINK_MESSAGE_INFO_SIM_STATE { \ + "SIM_STATE", \ + 21, \ + { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \ + { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \ + { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \ + { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sim_state_t, q4) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sim_state_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sim_state_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sim_state_t, yaw) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sim_state_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sim_state_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_sim_state_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_sim_state_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_sim_state_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_sim_state_t, zgyro) }, \ + { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_sim_state_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_sim_state_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_sim_state_t, alt) }, \ + { "std_dev_horz", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_sim_state_t, std_dev_horz) }, \ + { "std_dev_vert", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_sim_state_t, std_dev_vert) }, \ + { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \ + { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \ + { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \ + } \ +} + + +/** + * @brief Pack a sim_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) + * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param std_dev_horz Horizontal position standard deviation + * @param std_dev_vert Vertical position standard deviation + * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame + * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame + * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#else + mavlink_sim_state_t packet; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.std_dev_horz = std_dev_horz; + packet.std_dev_vert = std_dev_vert; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SIM_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +} + +/** + * @brief Pack a sim_state message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) + * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param std_dev_horz Horizontal position standard deviation + * @param std_dev_vert Vertical position standard deviation + * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame + * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame + * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float q1,float q2,float q3,float q4,float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lon,float alt,float std_dev_horz,float std_dev_vert,float vn,float ve,float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#else + mavlink_sim_state_t packet; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.std_dev_horz = std_dev_horz; + packet.std_dev_vert = std_dev_vert; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SIM_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +} + +/** + * @brief Encode a sim_state struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); +} + +/** + * @brief Encode a sim_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); +} + +/** + * @brief Send a sim_state message + * @param chan MAVLink channel to send the message + * + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) + * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param xacc X acceleration m/s/s + * @param yacc Y acceleration m/s/s + * @param zacc Z acceleration m/s/s + * @param xgyro Angular speed around X axis rad/s + * @param ygyro Angular speed around Y axis rad/s + * @param zgyro Angular speed around Z axis rad/s + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param std_dev_horz Horizontal position standard deviation + * @param std_dev_vert Vertical position standard deviation + * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame + * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame + * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#else + mavlink_sim_state_t packet; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.std_dev_horz = std_dev_horz; + packet.std_dev_vert = std_dev_vert; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#else + mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->std_dev_horz = std_dev_horz; + packet->std_dev_vert = std_dev_vert; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SIM_STATE UNPACKING + + +/** + * @brief Get field q1 from sim_state message + * + * @return True attitude quaternion component 1, w (1 in null-rotation) + */ +static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field q2 from sim_state message + * + * @return True attitude quaternion component 2, x (0 in null-rotation) + */ +static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field q3 from sim_state message + * + * @return True attitude quaternion component 3, y (0 in null-rotation) + */ +static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field q4 from sim_state message + * + * @return True attitude quaternion component 4, z (0 in null-rotation) + */ +static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field roll from sim_state message + * + * @return Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + */ +static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field pitch from sim_state message + * + * @return Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + */ +static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field yaw from sim_state message + * + * @return Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + */ +static inline float mavlink_msg_sim_state_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field xacc from sim_state message + * + * @return X acceleration m/s/s + */ +static inline float mavlink_msg_sim_state_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field yacc from sim_state message + * + * @return Y acceleration m/s/s + */ +static inline float mavlink_msg_sim_state_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field zacc from sim_state message + * + * @return Z acceleration m/s/s + */ +static inline float mavlink_msg_sim_state_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field xgyro from sim_state message + * + * @return Angular speed around X axis rad/s + */ +static inline float mavlink_msg_sim_state_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field ygyro from sim_state message + * + * @return Angular speed around Y axis rad/s + */ +static inline float mavlink_msg_sim_state_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field zgyro from sim_state message + * + * @return Angular speed around Z axis rad/s + */ +static inline float mavlink_msg_sim_state_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field lat from sim_state message + * + * @return Latitude in degrees + */ +static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field lon from sim_state message + * + * @return Longitude in degrees + */ +static inline float mavlink_msg_sim_state_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field alt from sim_state message + * + * @return Altitude in meters + */ +static inline float mavlink_msg_sim_state_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 60); +} + +/** + * @brief Get field std_dev_horz from sim_state message + * + * @return Horizontal position standard deviation + */ +static inline float mavlink_msg_sim_state_get_std_dev_horz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 64); +} + +/** + * @brief Get field std_dev_vert from sim_state message + * + * @return Vertical position standard deviation + */ +static inline float mavlink_msg_sim_state_get_std_dev_vert(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 68); +} + +/** + * @brief Get field vn from sim_state message + * + * @return True velocity in m/s in NORTH direction in earth-fixed NED frame + */ +static inline float mavlink_msg_sim_state_get_vn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 72); +} + +/** + * @brief Get field ve from sim_state message + * + * @return True velocity in m/s in EAST direction in earth-fixed NED frame + */ +static inline float mavlink_msg_sim_state_get_ve(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 76); +} + +/** + * @brief Get field vd from sim_state message + * + * @return True velocity in m/s in DOWN direction in earth-fixed NED frame + */ +static inline float mavlink_msg_sim_state_get_vd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 80); +} + +/** + * @brief Decode a sim_state message into a struct + * + * @param msg The message to decode + * @param sim_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_sim_state_decode(const mavlink_message_t* msg, mavlink_sim_state_t* sim_state) +{ +#if MAVLINK_NEED_BYTE_SWAP + sim_state->q1 = mavlink_msg_sim_state_get_q1(msg); + sim_state->q2 = mavlink_msg_sim_state_get_q2(msg); + sim_state->q3 = mavlink_msg_sim_state_get_q3(msg); + sim_state->q4 = mavlink_msg_sim_state_get_q4(msg); + sim_state->roll = mavlink_msg_sim_state_get_roll(msg); + sim_state->pitch = mavlink_msg_sim_state_get_pitch(msg); + sim_state->yaw = mavlink_msg_sim_state_get_yaw(msg); + sim_state->xacc = mavlink_msg_sim_state_get_xacc(msg); + sim_state->yacc = mavlink_msg_sim_state_get_yacc(msg); + sim_state->zacc = mavlink_msg_sim_state_get_zacc(msg); + sim_state->xgyro = mavlink_msg_sim_state_get_xgyro(msg); + sim_state->ygyro = mavlink_msg_sim_state_get_ygyro(msg); + sim_state->zgyro = mavlink_msg_sim_state_get_zgyro(msg); + sim_state->lat = mavlink_msg_sim_state_get_lat(msg); + sim_state->lon = mavlink_msg_sim_state_get_lon(msg); + sim_state->alt = mavlink_msg_sim_state_get_alt(msg); + sim_state->std_dev_horz = mavlink_msg_sim_state_get_std_dev_horz(msg); + sim_state->std_dev_vert = mavlink_msg_sim_state_get_std_dev_vert(msg); + sim_state->vn = mavlink_msg_sim_state_get_vn(msg); + sim_state->ve = mavlink_msg_sim_state_get_ve(msg); + sim_state->vd = mavlink_msg_sim_state_get_vd(msg); +#else + memcpy(sim_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_statustext.h b/flightcode/mavlink/c_library/common/mavlink_msg_statustext.h new file mode 100644 index 0000000..c8c2547 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_statustext.h @@ -0,0 +1,225 @@ +// MESSAGE STATUSTEXT PACKING + +#define MAVLINK_MSG_ID_STATUSTEXT 253 + +typedef struct __mavlink_statustext_t +{ + uint8_t severity; ///< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + char text[50]; ///< Status text message, without null termination character +} mavlink_statustext_t; + +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_253_LEN 51 + +#define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 +#define MAVLINK_MSG_ID_253_CRC 83 + +#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 + +#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ + "STATUSTEXT", \ + 2, \ + { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ + { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + } \ +} + + +/** + * @brief Pack a statustext message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +} + +/** + * @brief Pack a statustext message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t severity,const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +} + +/** + * @brief Encode a statustext struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); +} + +/** + * @brief Encode a statustext struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); +} + +/** + * @brief Send a statustext message + * @param chan MAVLink channel to send the message + * + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(char)*50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#else + mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; + packet->severity = severity; + mav_array_memcpy(packet->text, text, sizeof(char)*50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE STATUSTEXT UNPACKING + + +/** + * @brief Get field severity from statustext message + * + * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + */ +static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field text from statustext message + * + * @return Status text message, without null termination character + */ +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) +{ + return _MAV_RETURN_char_array(msg, text, 50, 1); +} + +/** + * @brief Decode a statustext message into a struct + * + * @param msg The message to decode + * @param statustext C-struct to decode the message contents into + */ +static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) +{ +#if MAVLINK_NEED_BYTE_SWAP + statustext->severity = mavlink_msg_statustext_get_severity(msg); + mavlink_msg_statustext_get_text(msg, statustext->text); +#else + memcpy(statustext, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_sys_status.h b/flightcode/mavlink/c_library/common/mavlink_msg_sys_status.h new file mode 100644 index 0000000..1b1730d --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_sys_status.h @@ -0,0 +1,497 @@ +// MESSAGE SYS_STATUS PACKING + +#define MAVLINK_MSG_ID_SYS_STATUS 1 + +typedef struct __mavlink_sys_status_t +{ + uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) + int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + uint16_t drop_rate_comm; ///< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + uint16_t errors_comm; ///< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + uint16_t errors_count1; ///< Autopilot-specific errors + uint16_t errors_count2; ///< Autopilot-specific errors + uint16_t errors_count3; ///< Autopilot-specific errors + uint16_t errors_count4; ///< Autopilot-specific errors + int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery +} mavlink_sys_status_t; + +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 31 +#define MAVLINK_MSG_ID_1_LEN 31 + +#define MAVLINK_MSG_ID_SYS_STATUS_CRC 124 +#define MAVLINK_MSG_ID_1_CRC 124 + + + +#define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ + "SYS_STATUS", \ + 13, \ + { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ + { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ + { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ + { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, load) }, \ + { "voltage_battery", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \ + { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \ + { "drop_rate_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \ + { "errors_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \ + { "errors_count1", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \ + { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \ + { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \ + { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \ + { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \ + } \ +} + + +/** + * @brief Pack a sys_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_count1 Autopilot-specific errors + * @param errors_count2 Autopilot-specific errors + * @param errors_count3 Autopilot-specific errors + * @param errors_count4 Autopilot-specific errors + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +} + +/** + * @brief Pack a sys_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_count1 Autopilot-specific errors + * @param errors_count2 Autopilot-specific errors + * @param errors_count3 Autopilot-specific errors + * @param errors_count4 Autopilot-specific errors + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +} + +/** + * @brief Encode a sys_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); +} + +/** + * @brief Encode a sys_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); +} + +/** + * @brief Send a sys_status message + * @param chan MAVLink channel to send the message + * + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_count1 Autopilot-specific errors + * @param errors_count2 Autopilot-specific errors + * @param errors_count3 Autopilot-specific errors + * @param errors_count4 Autopilot-specific errors + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#else + mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf; + packet->onboard_control_sensors_present = onboard_control_sensors_present; + packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet->onboard_control_sensors_health = onboard_control_sensors_health; + packet->load = load; + packet->voltage_battery = voltage_battery; + packet->current_battery = current_battery; + packet->drop_rate_comm = drop_rate_comm; + packet->errors_comm = errors_comm; + packet->errors_count1 = errors_count1; + packet->errors_count2 = errors_count2; + packet->errors_count3 = errors_count3; + packet->errors_count4 = errors_count4; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SYS_STATUS UNPACKING + + +/** + * @brief Get field onboard_control_sensors_present from sys_status message + * + * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field onboard_control_sensors_enabled from sys_status message + * + * @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field onboard_control_sensors_health from sys_status message + * + * @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field load from sys_status message + * + * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + */ +static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field voltage_battery from sys_status message + * + * @return Battery voltage, in millivolts (1 = 1 millivolt) + */ +static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field current_battery from sys_status message + * + * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + */ +static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field battery_remaining from sys_status message + * + * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + */ +static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 30); +} + +/** + * @brief Get field drop_rate_comm from sys_status message + * + * @return Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + */ +static inline uint16_t mavlink_msg_sys_status_get_drop_rate_comm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field errors_comm from sys_status message + * + * @return Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + */ +static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field errors_count1 from sys_status message + * + * @return Autopilot-specific errors + */ +static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field errors_count2 from sys_status message + * + * @return Autopilot-specific errors + */ +static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field errors_count3 from sys_status message + * + * @return Autopilot-specific errors + */ +static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field errors_count4 from sys_status message + * + * @return Autopilot-specific errors + */ +static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Decode a sys_status message into a struct + * + * @param msg The message to decode + * @param sys_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + sys_status->onboard_control_sensors_present = mavlink_msg_sys_status_get_onboard_control_sensors_present(msg); + sys_status->onboard_control_sensors_enabled = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(msg); + sys_status->onboard_control_sensors_health = mavlink_msg_sys_status_get_onboard_control_sensors_health(msg); + sys_status->load = mavlink_msg_sys_status_get_load(msg); + sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg); + sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg); + sys_status->drop_rate_comm = mavlink_msg_sys_status_get_drop_rate_comm(msg); + sys_status->errors_comm = mavlink_msg_sys_status_get_errors_comm(msg); + sys_status->errors_count1 = mavlink_msg_sys_status_get_errors_count1(msg); + sys_status->errors_count2 = mavlink_msg_sys_status_get_errors_count2(msg); + sys_status->errors_count3 = mavlink_msg_sys_status_get_errors_count3(msg); + sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg); + sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); +#else + memcpy(sys_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_system_time.h b/flightcode/mavlink/c_library/common/mavlink_msg_system_time.h new file mode 100644 index 0000000..988c669 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_system_time.h @@ -0,0 +1,233 @@ +// MESSAGE SYSTEM_TIME PACKING + +#define MAVLINK_MSG_ID_SYSTEM_TIME 2 + +typedef struct __mavlink_system_time_t +{ + uint64_t time_unix_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds. +} mavlink_system_time_t; + +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12 +#define MAVLINK_MSG_ID_2_LEN 12 + +#define MAVLINK_MSG_ID_SYSTEM_TIME_CRC 137 +#define MAVLINK_MSG_ID_2_CRC 137 + + + +#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \ + "SYSTEM_TIME", \ + 2, \ + { { "time_unix_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_unix_usec) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_system_time_t, time_boot_ms) }, \ + } \ +} + + +/** + * @brief Pack a system_time message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. + * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +} + +/** + * @brief Pack a system_time message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. + * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_unix_usec,uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +} + +/** + * @brief Encode a system_time struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + +/** + * @brief Encode a system_time struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + +/** + * @brief Send a system_time message + * @param chan MAVLink channel to send the message + * + * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. + * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#else + mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf; + packet->time_unix_usec = time_unix_usec; + packet->time_boot_ms = time_boot_ms; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SYSTEM_TIME UNPACKING + + +/** + * @brief Get field time_unix_usec from system_time message + * + * @return Timestamp of the master clock in microseconds since UNIX epoch. + */ +static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field time_boot_ms from system_time message + * + * @return Timestamp of the component clock since boot time in milliseconds. + */ +static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Decode a system_time message into a struct + * + * @param msg The message to decode + * @param system_time C-struct to decode the message contents into + */ +static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) +{ +#if MAVLINK_NEED_BYTE_SWAP + system_time->time_unix_usec = mavlink_msg_system_time_get_time_unix_usec(msg); + system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg); +#else + memcpy(system_time, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_terrain_check.h b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_check.h new file mode 100644 index 0000000..ebdf71a --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_check.h @@ -0,0 +1,233 @@ +// MESSAGE TERRAIN_CHECK PACKING + +#define MAVLINK_MSG_ID_TERRAIN_CHECK 135 + +typedef struct __mavlink_terrain_check_t +{ + int32_t lat; ///< Latitude (degrees *10^7) + int32_t lon; ///< Longitude (degrees *10^7) +} mavlink_terrain_check_t; + +#define MAVLINK_MSG_ID_TERRAIN_CHECK_LEN 8 +#define MAVLINK_MSG_ID_135_LEN 8 + +#define MAVLINK_MSG_ID_TERRAIN_CHECK_CRC 203 +#define MAVLINK_MSG_ID_135_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_CHECK { \ + "TERRAIN_CHECK", \ + 2, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_check_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_check_t, lon) }, \ + } \ +} + + +/** + * @brief Pack a terrain_check message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_check_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} + +/** + * @brief Pack a terrain_check message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_check_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} + +/** + * @brief Encode a terrain_check struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_check C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_check_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) +{ + return mavlink_msg_terrain_check_pack(system_id, component_id, msg, terrain_check->lat, terrain_check->lon); +} + +/** + * @brief Encode a terrain_check struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_check C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_check_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) +{ + return mavlink_msg_terrain_check_pack_chan(system_id, component_id, chan, msg, terrain_check->lat, terrain_check->lon); +} + +/** + * @brief Send a terrain_check message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_check_send(mavlink_channel_t chan, int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_CHECK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_check_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#else + mavlink_terrain_check_t *packet = (mavlink_terrain_check_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_CHECK, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_CHECK UNPACKING + + +/** + * @brief Get field lat from terrain_check message + * + * @return Latitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_check_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_check message + * + * @return Longitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_check_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Decode a terrain_check message into a struct + * + * @param msg The message to decode + * @param terrain_check C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_check_decode(const mavlink_message_t* msg, mavlink_terrain_check_t* terrain_check) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_check->lat = mavlink_msg_terrain_check_get_lat(msg); + terrain_check->lon = mavlink_msg_terrain_check_get_lon(msg); +#else + memcpy(terrain_check, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_terrain_data.h b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_data.h new file mode 100644 index 0000000..8fc0b43 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_data.h @@ -0,0 +1,297 @@ +// MESSAGE TERRAIN_DATA PACKING + +#define MAVLINK_MSG_ID_TERRAIN_DATA 134 + +typedef struct __mavlink_terrain_data_t +{ + int32_t lat; ///< Latitude of SW corner of first grid (degrees *10^7) + int32_t lon; ///< Longitude of SW corner of first grid (in degrees *10^7) + uint16_t grid_spacing; ///< Grid spacing in meters + int16_t data[16]; ///< Terrain data in meters AMSL + uint8_t gridbit; ///< bit within the terrain request mask +} mavlink_terrain_data_t; + +#define MAVLINK_MSG_ID_TERRAIN_DATA_LEN 43 +#define MAVLINK_MSG_ID_134_LEN 43 + +#define MAVLINK_MSG_ID_TERRAIN_DATA_CRC 229 +#define MAVLINK_MSG_ID_134_CRC 229 + +#define MAVLINK_MSG_TERRAIN_DATA_FIELD_DATA_LEN 16 + +#define MAVLINK_MESSAGE_INFO_TERRAIN_DATA { \ + "TERRAIN_DATA", \ + 5, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_data_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_data_t, lon) }, \ + { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_terrain_data_t, grid_spacing) }, \ + { "data", NULL, MAVLINK_TYPE_INT16_T, 16, 10, offsetof(mavlink_terrain_data_t, data) }, \ + { "gridbit", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_terrain_data_t, gridbit) }, \ + } \ +} + + +/** + * @brief Pack a terrain_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} + +/** + * @brief Pack a terrain_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t grid_spacing,uint8_t gridbit,const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} + +/** + * @brief Encode a terrain_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data) +{ + return mavlink_msg_terrain_data_pack(system_id, component_id, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); +} + +/** + * @brief Encode a terrain_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data) +{ + return mavlink_msg_terrain_data_pack_chan(system_id, component_id, chan, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); +} + +/** + * @brief Send a terrain_data message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param gridbit bit within the terrain request mask + * @param data Terrain data in meters AMSL + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_data_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#else + mavlink_terrain_data_t *packet = (mavlink_terrain_data_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->grid_spacing = grid_spacing; + packet->gridbit = gridbit; + mav_array_memcpy(packet->data, data, sizeof(int16_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_DATA UNPACKING + + +/** + * @brief Get field lat from terrain_data message + * + * @return Latitude of SW corner of first grid (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_data_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_data message + * + * @return Longitude of SW corner of first grid (in degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_data_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field grid_spacing from terrain_data message + * + * @return Grid spacing in meters + */ +static inline uint16_t mavlink_msg_terrain_data_get_grid_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field gridbit from terrain_data message + * + * @return bit within the terrain request mask + */ +static inline uint8_t mavlink_msg_terrain_data_get_gridbit(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field data from terrain_data message + * + * @return Terrain data in meters AMSL + */ +static inline uint16_t mavlink_msg_terrain_data_get_data(const mavlink_message_t* msg, int16_t *data) +{ + return _MAV_RETURN_int16_t_array(msg, data, 16, 10); +} + +/** + * @brief Decode a terrain_data message into a struct + * + * @param msg The message to decode + * @param terrain_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_data_decode(const mavlink_message_t* msg, mavlink_terrain_data_t* terrain_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_data->lat = mavlink_msg_terrain_data_get_lat(msg); + terrain_data->lon = mavlink_msg_terrain_data_get_lon(msg); + terrain_data->grid_spacing = mavlink_msg_terrain_data_get_grid_spacing(msg); + mavlink_msg_terrain_data_get_data(msg, terrain_data->data); + terrain_data->gridbit = mavlink_msg_terrain_data_get_gridbit(msg); +#else + memcpy(terrain_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_terrain_report.h b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_report.h new file mode 100644 index 0000000..1b61dd4 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_report.h @@ -0,0 +1,353 @@ +// MESSAGE TERRAIN_REPORT PACKING + +#define MAVLINK_MSG_ID_TERRAIN_REPORT 136 + +typedef struct __mavlink_terrain_report_t +{ + int32_t lat; ///< Latitude (degrees *10^7) + int32_t lon; ///< Longitude (degrees *10^7) + float terrain_height; ///< Terrain height in meters AMSL + float current_height; ///< Current vehicle height above lat/lon terrain height (meters) + uint16_t spacing; ///< grid spacing (zero if terrain at this location unavailable) + uint16_t pending; ///< Number of 4x4 terrain blocks waiting to be received or read from disk + uint16_t loaded; ///< Number of 4x4 terrain blocks in memory +} mavlink_terrain_report_t; + +#define MAVLINK_MSG_ID_TERRAIN_REPORT_LEN 22 +#define MAVLINK_MSG_ID_136_LEN 22 + +#define MAVLINK_MSG_ID_TERRAIN_REPORT_CRC 1 +#define MAVLINK_MSG_ID_136_CRC 1 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_REPORT { \ + "TERRAIN_REPORT", \ + 7, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_report_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_report_t, lon) }, \ + { "terrain_height", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_terrain_report_t, terrain_height) }, \ + { "current_height", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_terrain_report_t, current_height) }, \ + { "spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_terrain_report_t, spacing) }, \ + { "pending", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_terrain_report_t, pending) }, \ + { "loaded", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_terrain_report_t, loaded) }, \ + } \ +} + + +/** + * @brief Pack a terrain_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} + +/** + * @brief Pack a terrain_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t spacing,float terrain_height,float current_height,uint16_t pending,uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} + +/** + * @brief Encode a terrain_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_report_t* terrain_report) +{ + return mavlink_msg_terrain_report_pack(system_id, component_id, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); +} + +/** + * @brief Encode a terrain_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_report_t* terrain_report) +{ + return mavlink_msg_terrain_report_pack_chan(system_id, component_id, chan, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); +} + +/** + * @brief Send a terrain_report message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude (degrees *10^7) + * @param lon Longitude (degrees *10^7) + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height Terrain height in meters AMSL + * @param current_height Current vehicle height above lat/lon terrain height (meters) + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_report_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#else + mavlink_terrain_report_t *packet = (mavlink_terrain_report_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->terrain_height = terrain_height; + packet->current_height = current_height; + packet->spacing = spacing; + packet->pending = pending; + packet->loaded = loaded; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REPORT, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_REPORT UNPACKING + + +/** + * @brief Get field lat from terrain_report message + * + * @return Latitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_report_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from terrain_report message + * + * @return Longitude (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_report_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field spacing from terrain_report message + * + * @return grid spacing (zero if terrain at this location unavailable) + */ +static inline uint16_t mavlink_msg_terrain_report_get_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field terrain_height from terrain_report message + * + * @return Terrain height in meters AMSL + */ +static inline float mavlink_msg_terrain_report_get_terrain_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field current_height from terrain_report message + * + * @return Current vehicle height above lat/lon terrain height (meters) + */ +static inline float mavlink_msg_terrain_report_get_current_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field pending from terrain_report message + * + * @return Number of 4x4 terrain blocks waiting to be received or read from disk + */ +static inline uint16_t mavlink_msg_terrain_report_get_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field loaded from terrain_report message + * + * @return Number of 4x4 terrain blocks in memory + */ +static inline uint16_t mavlink_msg_terrain_report_get_loaded(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Decode a terrain_report message into a struct + * + * @param msg The message to decode + * @param terrain_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_report_decode(const mavlink_message_t* msg, mavlink_terrain_report_t* terrain_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_report->lat = mavlink_msg_terrain_report_get_lat(msg); + terrain_report->lon = mavlink_msg_terrain_report_get_lon(msg); + terrain_report->terrain_height = mavlink_msg_terrain_report_get_terrain_height(msg); + terrain_report->current_height = mavlink_msg_terrain_report_get_current_height(msg); + terrain_report->spacing = mavlink_msg_terrain_report_get_spacing(msg); + terrain_report->pending = mavlink_msg_terrain_report_get_pending(msg); + terrain_report->loaded = mavlink_msg_terrain_report_get_loaded(msg); +#else + memcpy(terrain_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_terrain_request.h b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_request.h new file mode 100644 index 0000000..3a85b85 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_terrain_request.h @@ -0,0 +1,281 @@ +// MESSAGE TERRAIN_REQUEST PACKING + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST 133 + +typedef struct __mavlink_terrain_request_t +{ + uint64_t mask; ///< Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + int32_t lat; ///< Latitude of SW corner of first grid (degrees *10^7) + int32_t lon; ///< Longitude of SW corner of first grid (in degrees *10^7) + uint16_t grid_spacing; ///< Grid spacing in meters +} mavlink_terrain_request_t; + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN 18 +#define MAVLINK_MSG_ID_133_LEN 18 + +#define MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC 6 +#define MAVLINK_MSG_ID_133_CRC 6 + + + +#define MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST { \ + "TERRAIN_REQUEST", \ + 4, \ + { { "mask", "0x%07x", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_terrain_request_t, mask) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_terrain_request_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_terrain_request_t, lon) }, \ + { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_terrain_request_t, grid_spacing) }, \ + } \ +} + + +/** + * @brief Pack a terrain_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} + +/** + * @brief Pack a terrain_request message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,uint16_t grid_spacing,uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} + +/** + * @brief Encode a terrain_request struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param terrain_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_request_t* terrain_request) +{ + return mavlink_msg_terrain_request_pack(system_id, component_id, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); +} + +/** + * @brief Encode a terrain_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param terrain_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_request_t* terrain_request) +{ + return mavlink_msg_terrain_request_pack_chan(system_id, component_id, chan, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); +} + +/** + * @brief Send a terrain_request message + * @param chan MAVLink channel to send the message + * + * @param lat Latitude of SW corner of first grid (degrees *10^7) + * @param lon Longitude of SW corner of first grid (in degrees *10^7) + * @param grid_spacing Grid spacing in meters + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_terrain_request_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_terrain_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#else + mavlink_terrain_request_t *packet = (mavlink_terrain_request_t *)msgbuf; + packet->mask = mask; + packet->lat = lat; + packet->lon = lon; + packet->grid_spacing = grid_spacing; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_REQUEST, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TERRAIN_REQUEST UNPACKING + + +/** + * @brief Get field lat from terrain_request message + * + * @return Latitude of SW corner of first grid (degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_request_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from terrain_request message + * + * @return Longitude of SW corner of first grid (in degrees *10^7) + */ +static inline int32_t mavlink_msg_terrain_request_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field grid_spacing from terrain_request message + * + * @return Grid spacing in meters + */ +static inline uint16_t mavlink_msg_terrain_request_get_grid_spacing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field mask from terrain_request message + * + * @return Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + */ +static inline uint64_t mavlink_msg_terrain_request_get_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Decode a terrain_request message into a struct + * + * @param msg The message to decode + * @param terrain_request C-struct to decode the message contents into + */ +static inline void mavlink_msg_terrain_request_decode(const mavlink_message_t* msg, mavlink_terrain_request_t* terrain_request) +{ +#if MAVLINK_NEED_BYTE_SWAP + terrain_request->mask = mavlink_msg_terrain_request_get_mask(msg); + terrain_request->lat = mavlink_msg_terrain_request_get_lat(msg); + terrain_request->lon = mavlink_msg_terrain_request_get_lon(msg); + terrain_request->grid_spacing = mavlink_msg_terrain_request_get_grid_spacing(msg); +#else + memcpy(terrain_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_timesync.h b/flightcode/mavlink/c_library/common/mavlink_msg_timesync.h new file mode 100644 index 0000000..6f1d5e1 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_timesync.h @@ -0,0 +1,233 @@ +// MESSAGE TIMESYNC PACKING + +#define MAVLINK_MSG_ID_TIMESYNC 111 + +typedef struct __mavlink_timesync_t +{ + int64_t tc1; ///< Time sync timestamp 1 + int64_t ts1; ///< Time sync timestamp 2 +} mavlink_timesync_t; + +#define MAVLINK_MSG_ID_TIMESYNC_LEN 16 +#define MAVLINK_MSG_ID_111_LEN 16 + +#define MAVLINK_MSG_ID_TIMESYNC_CRC 34 +#define MAVLINK_MSG_ID_111_CRC 34 + + + +#define MAVLINK_MESSAGE_INFO_TIMESYNC { \ + "TIMESYNC", \ + 2, \ + { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ + { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ + } \ +} + + +/** + * @brief Pack a timesync message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int64_t tc1, int64_t ts1) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#else + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +} + +/** + * @brief Pack a timesync message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int64_t tc1,int64_t ts1) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#else + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +} + +/** + * @brief Encode a timesync struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param timesync C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_timesync_t* timesync) +{ + return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1); +} + +/** + * @brief Encode a timesync struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param timesync C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_timesync_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_timesync_t* timesync) +{ + return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1); +} + +/** + * @brief Send a timesync message + * @param chan MAVLink channel to send the message + * + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1, int64_t ts1) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +#else + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_TIMESYNC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t tc1, int64_t ts1) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +#else + mavlink_timesync_t *packet = (mavlink_timesync_t *)msgbuf; + packet->tc1 = tc1; + packet->ts1 = ts1; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE TIMESYNC UNPACKING + + +/** + * @brief Get field tc1 from timesync message + * + * @return Time sync timestamp 1 + */ +static inline int64_t mavlink_msg_timesync_get_tc1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int64_t(msg, 0); +} + +/** + * @brief Get field ts1 from timesync message + * + * @return Time sync timestamp 2 + */ +static inline int64_t mavlink_msg_timesync_get_ts1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int64_t(msg, 8); +} + +/** + * @brief Decode a timesync message into a struct + * + * @param msg The message to decode + * @param timesync C-struct to decode the message contents into + */ +static inline void mavlink_msg_timesync_decode(const mavlink_message_t* msg, mavlink_timesync_t* timesync) +{ +#if MAVLINK_NEED_BYTE_SWAP + timesync->tc1 = mavlink_msg_timesync_get_tc1(msg); + timesync->ts1 = mavlink_msg_timesync_get_ts1(msg); +#else + memcpy(timesync, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_v2_extension.h b/flightcode/mavlink/c_library/common/mavlink_msg_v2_extension.h new file mode 100644 index 0000000..59a0ba0 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_v2_extension.h @@ -0,0 +1,297 @@ +// MESSAGE V2_EXTENSION PACKING + +#define MAVLINK_MSG_ID_V2_EXTENSION 248 + +typedef struct __mavlink_v2_extension_t +{ + uint16_t message_type; ///< A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + uint8_t target_network; ///< Network ID (0 for broadcast) + uint8_t target_system; ///< System ID (0 for broadcast) + uint8_t target_component; ///< Component ID (0 for broadcast) + uint8_t payload[249]; ///< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. +} mavlink_v2_extension_t; + +#define MAVLINK_MSG_ID_V2_EXTENSION_LEN 254 +#define MAVLINK_MSG_ID_248_LEN 254 + +#define MAVLINK_MSG_ID_V2_EXTENSION_CRC 8 +#define MAVLINK_MSG_ID_248_CRC 8 + +#define MAVLINK_MSG_V2_EXTENSION_FIELD_PAYLOAD_LEN 249 + +#define MAVLINK_MESSAGE_INFO_V2_EXTENSION { \ + "V2_EXTENSION", \ + 5, \ + { { "message_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_v2_extension_t, message_type) }, \ + { "target_network", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_v2_extension_t, target_network) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_v2_extension_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_v2_extension_t, target_component) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 249, 5, offsetof(mavlink_v2_extension_t, payload) }, \ + } \ +} + + +/** + * @brief Pack a v2_extension message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_v2_extension_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_V2_EXTENSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} + +/** + * @brief Pack a v2_extension message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_v2_extension_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_network,uint8_t target_system,uint8_t target_component,uint16_t message_type,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_V2_EXTENSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} + +/** + * @brief Encode a v2_extension struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param v2_extension C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_v2_extension_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_v2_extension_t* v2_extension) +{ + return mavlink_msg_v2_extension_pack(system_id, component_id, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); +} + +/** + * @brief Encode a v2_extension struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param v2_extension C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_v2_extension_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_v2_extension_t* v2_extension) +{ + return mavlink_msg_v2_extension_pack_chan(system_id, component_id, chan, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); +} + +/** + * @brief Send a v2_extension message + * @param chan MAVLink channel to send the message + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_v2_extension_send(mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)&packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)&packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_V2_EXTENSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_v2_extension_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#else + mavlink_v2_extension_t *packet = (mavlink_v2_extension_t *)msgbuf; + packet->message_type = message_type; + packet->target_network = target_network; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*249); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE V2_EXTENSION UNPACKING + + +/** + * @brief Get field target_network from v2_extension message + * + * @return Network ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_network(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_system from v2_extension message + * + * @return System ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field target_component from v2_extension message + * + * @return Component ID (0 for broadcast) + */ +static inline uint8_t mavlink_msg_v2_extension_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field message_type from v2_extension message + * + * @return A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + */ +static inline uint16_t mavlink_msg_v2_extension_get_message_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field payload from v2_extension message + * + * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + */ +static inline uint16_t mavlink_msg_v2_extension_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 249, 5); +} + +/** + * @brief Decode a v2_extension message into a struct + * + * @param msg The message to decode + * @param v2_extension C-struct to decode the message contents into + */ +static inline void mavlink_msg_v2_extension_decode(const mavlink_message_t* msg, mavlink_v2_extension_t* v2_extension) +{ +#if MAVLINK_NEED_BYTE_SWAP + v2_extension->message_type = mavlink_msg_v2_extension_get_message_type(msg); + v2_extension->target_network = mavlink_msg_v2_extension_get_target_network(msg); + v2_extension->target_system = mavlink_msg_v2_extension_get_target_system(msg); + v2_extension->target_component = mavlink_msg_v2_extension_get_target_component(msg); + mavlink_msg_v2_extension_get_payload(msg, v2_extension->payload); +#else + memcpy(v2_extension, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_vfr_hud.h b/flightcode/mavlink/c_library/common/mavlink_msg_vfr_hud.h new file mode 100644 index 0000000..b130ee5 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_vfr_hud.h @@ -0,0 +1,329 @@ +// MESSAGE VFR_HUD PACKING + +#define MAVLINK_MSG_ID_VFR_HUD 74 + +typedef struct __mavlink_vfr_hud_t +{ + float airspeed; ///< Current airspeed in m/s + float groundspeed; ///< Current ground speed in m/s + float alt; ///< Current altitude (MSL), in meters + float climb; ///< Current climb rate in meters/second + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 +} mavlink_vfr_hud_t; + +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_ID_74_LEN 20 + +#define MAVLINK_MSG_ID_VFR_HUD_CRC 20 +#define MAVLINK_MSG_ID_74_CRC 20 + + + +#define MAVLINK_MESSAGE_INFO_VFR_HUD { \ + "VFR_HUD", \ + 6, \ + { { "airspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \ + { "groundspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vfr_hud_t, alt) }, \ + { "climb", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, climb) }, \ + { "heading", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_vfr_hud_t, heading) }, \ + { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vfr_hud_t, throttle) }, \ + } \ +} + + +/** + * @brief Pack a vfr_hud message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +} + +/** + * @brief Pack a vfr_hud message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +} + +/** + * @brief Encode a vfr_hud struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + +/** + * @brief Encode a vfr_hud struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + +/** + * @brief Send a vfr_hud message + * @param chan MAVLink channel to send the message + * + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#else + mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf; + packet->airspeed = airspeed; + packet->groundspeed = groundspeed; + packet->alt = alt; + packet->climb = climb; + packet->heading = heading; + packet->throttle = throttle; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE VFR_HUD UNPACKING + + +/** + * @brief Get field airspeed from vfr_hud message + * + * @return Current airspeed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field groundspeed from vfr_hud message + * + * @return Current ground speed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field heading from vfr_hud message + * + * @return Current heading in degrees, in compass units (0..360, 0=north) + */ +static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field throttle from vfr_hud message + * + * @return Current throttle setting in integer percent, 0 to 100 + */ +static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field alt from vfr_hud message + * + * @return Current altitude (MSL), in meters + */ +static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field climb from vfr_hud message + * + * @return Current climb rate in meters/second + */ +static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a vfr_hud message into a struct + * + * @param msg The message to decode + * @param vfr_hud C-struct to decode the message contents into + */ +static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) +{ +#if MAVLINK_NEED_BYTE_SWAP + vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); + vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); + vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); + vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); + vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); + vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); +#else + memcpy(vfr_hud, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_vicon_position_estimate.h b/flightcode/mavlink/c_library/common/mavlink_msg_vicon_position_estimate.h new file mode 100644 index 0000000..b3fa7bc --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_vicon_position_estimate.h @@ -0,0 +1,353 @@ +// MESSAGE VICON_POSITION_ESTIMATE PACKING + +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 104 + +typedef struct __mavlink_vicon_position_estimate_t +{ + uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad +} mavlink_vicon_position_estimate_t; + +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_104_LEN 32 + +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC 56 +#define MAVLINK_MSG_ID_104_CRC 56 + + + +#define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ + "VICON_POSITION_ESTIMATE", \ + 7, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vicon_position_estimate_t, z) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a vicon_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Pack a vicon_position_estimate message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Encode a vicon_position_estimate struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); +} + +/** + * @brief Encode a vicon_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); +} + +/** + * @brief Send a vicon_position_estimate message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE VICON_POSITION_ESTIMATE UNPACKING + + +/** + * @brief Get field usec from vicon_position_estimate message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field x from vicon_position_estimate message + * + * @return Global X position + */ +static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from vicon_position_estimate message + * + * @return Global Y position + */ +static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from vicon_position_estimate message + * + * @return Global Z position + */ +static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field roll from vicon_position_estimate message + * + * @return Roll angle in rad + */ +static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitch from vicon_position_estimate message + * + * @return Pitch angle in rad + */ +static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw from vicon_position_estimate message + * + * @return Yaw angle in rad + */ +static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a vicon_position_estimate message into a struct + * + * @param msg The message to decode + * @param vicon_position_estimate C-struct to decode the message contents into + */ +static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ +#if MAVLINK_NEED_BYTE_SWAP + vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); + vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); + vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); + vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); + vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); + vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); + vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); +#else + memcpy(vicon_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_vision_position_estimate.h b/flightcode/mavlink/c_library/common/mavlink_msg_vision_position_estimate.h new file mode 100644 index 0000000..8f82fb6 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_vision_position_estimate.h @@ -0,0 +1,353 @@ +// MESSAGE VISION_POSITION_ESTIMATE PACKING + +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 102 + +typedef struct __mavlink_vision_position_estimate_t +{ + uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad +} mavlink_vision_position_estimate_t; + +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_102_LEN 32 + +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC 158 +#define MAVLINK_MSG_ID_102_CRC 158 + + + +#define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ + "VISION_POSITION_ESTIMATE", \ + 7, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a vision_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Pack a vision_position_estimate message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +} + +/** + * @brief Encode a vision_position_estimate struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); +} + +/** + * @brief Encode a vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); +} + +/** + * @brief Send a vision_position_estimate message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE VISION_POSITION_ESTIMATE UNPACKING + + +/** + * @brief Get field usec from vision_position_estimate message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field x from vision_position_estimate message + * + * @return Global X position + */ +static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from vision_position_estimate message + * + * @return Global Y position + */ +static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from vision_position_estimate message + * + * @return Global Z position + */ +static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field roll from vision_position_estimate message + * + * @return Roll angle in rad + */ +static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitch from vision_position_estimate message + * + * @return Pitch angle in rad + */ +static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw from vision_position_estimate message + * + * @return Yaw angle in rad + */ +static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a vision_position_estimate message into a struct + * + * @param msg The message to decode + * @param vision_position_estimate C-struct to decode the message contents into + */ +static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) +{ +#if MAVLINK_NEED_BYTE_SWAP + vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); + vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); + vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); + vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); + vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); + vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); + vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); +#else + memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/mavlink_msg_vision_speed_estimate.h b/flightcode/mavlink/c_library/common/mavlink_msg_vision_speed_estimate.h new file mode 100644 index 0000000..7528014 --- /dev/null +++ b/flightcode/mavlink/c_library/common/mavlink_msg_vision_speed_estimate.h @@ -0,0 +1,281 @@ +// MESSAGE VISION_SPEED_ESTIMATE PACKING + +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 103 + +typedef struct __mavlink_vision_speed_estimate_t +{ + uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float x; ///< Global X speed + float y; ///< Global Y speed + float z; ///< Global Z speed +} mavlink_vision_speed_estimate_t; + +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_ID_103_LEN 20 + +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC 208 +#define MAVLINK_MSG_ID_103_CRC 208 + + + +#define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ + "VISION_SPEED_ESTIMATE", \ + 4, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + } \ +} + + +/** + * @brief Pack a vision_speed_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X speed + * @param y Global Y speed + * @param z Global Z speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +} + +/** + * @brief Pack a vision_speed_estimate message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X speed + * @param y Global Y speed + * @param z Global Z speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +} + +/** + * @brief Encode a vision_speed_estimate struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); +} + +/** + * @brief Encode a vision_speed_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); +} + +/** + * @brief Send a vision_speed_estimate message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param x Global X speed + * @param y Global Y speed + * @param z Global Z speed + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#else + mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE VISION_SPEED_ESTIMATE UNPACKING + + +/** + * @brief Get field usec from vision_speed_estimate message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field x from vision_speed_estimate message + * + * @return Global X speed + */ +static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from vision_speed_estimate message + * + * @return Global Y speed + */ +static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from vision_speed_estimate message + * + * @return Global Z speed + */ +static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a vision_speed_estimate message into a struct + * + * @param msg The message to decode + * @param vision_speed_estimate C-struct to decode the message contents into + */ +static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ +#if MAVLINK_NEED_BYTE_SWAP + vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); + vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); + vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); + vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); +#else + memcpy(vision_speed_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +} diff --git a/flightcode/mavlink/c_library/common/testsuite.h b/flightcode/mavlink/c_library/common/testsuite.h new file mode 100644 index 0000000..df1ce58 --- /dev/null +++ b/flightcode/mavlink/c_library/common/testsuite.h @@ -0,0 +1,5904 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from common.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef COMMON_TESTSUITE_H +#define COMMON_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + + mavlink_test_common(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet_in = { + 963497464,17,84,151,218,3 + }; + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.base_mode = packet_in.base_mode; + packet1.system_status = packet_in.system_status; + packet1.mavlink_version = packet_in.mavlink_version; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i + +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + +/** + * @file mavlink_conversions.h + * + * These conversion functions follow the NASA rotation standards definition file + * available online. + * + * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free + * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) + * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the + * protocol as widely as possible. + * + * @author James Goppert + * @author Thomas Gubler + */ + + +/** + * Converts a quaternion to a rotation matrix + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param dcm a 3x3 rotation matrix + */ +MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) +{ + double a = quaternion[0]; + double b = quaternion[1]; + double c = quaternion[2]; + double d = quaternion[3]; + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2 * (b * c - a * d); + dcm[0][2] = 2 * (a * c + b * d); + dcm[1][0] = 2 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2 * (c * d - a * b); + dcm[2][0] = 2 * (b * d - a * c); + dcm[2][1] = 2 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; +} + + +/** + * Converts a rotation matrix to euler angles + * + * @param dcm a 3x3 rotation matrix + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ +MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) +{ + float phi, theta, psi; + theta = asin(-dcm[2][0]); + + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + *roll = phi; + *pitch = theta; + *yaw = psi; +} + + +/** + * Converts a quaternion to euler angles + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ +MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) +{ + float dcm[3][3]; + mavlink_quaternion_to_dcm(quaternion, dcm); + mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw); +} + + +/** + * Converts euler angles to a quaternion + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ +MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) +{ + float cosPhi_2 = cosf(roll / 2); + float sinPhi_2 = sinf(roll / 2); + float cosTheta_2 = cosf(pitch / 2); + float sinTheta_2 = sinf(pitch / 2); + float cosPsi_2 = cosf(yaw / 2); + float sinPsi_2 = sinf(yaw / 2); + quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + + +/** + * Converts a rotation matrix to a quaternion + * Reference: + * - Shoemake, Quaternions, + * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf + * + * @param dcm a 3x3 rotation matrix + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ +MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) +{ + float tr = dcm[0][0] + dcm[1][1] + dcm[2][2]; + if (tr > 0.0f) { + float s = sqrtf(tr + 1.0f); + quaternion[0] = s * 0.5f; + s = 0.5f / s; + quaternion[1] = (dcm[2][1] - dcm[1][2]) * s; + quaternion[2] = (dcm[0][2] - dcm[2][0]) * s; + quaternion[3] = (dcm[1][0] - dcm[0][1]) * s; + } else { + /* Find maximum diagonal element in dcm + * store index in dcm_i */ + int dcm_i = 0; + int i; + for (i = 1; i < 3; i++) { + if (dcm[i][i] > dcm[dcm_i][dcm_i]) { + dcm_i = i; + } + } + + int dcm_j = (dcm_i + 1) % 3; + int dcm_k = (dcm_i + 2) % 3; + + float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] - + dcm[dcm_k][dcm_k]) + 1.0f); + quaternion[dcm_i + 1] = s * 0.5f; + s = 0.5f / s; + quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s; + quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s; + quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s; + } +} + + +/** + * Converts euler angles to a rotation matrix + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param dcm a 3x3 rotation matrix + */ +MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) +{ + float cosPhi = cosf(roll); + float sinPhi = sinf(roll); + float cosThe = cosf(pitch); + float sinThe = sinf(pitch); + float cosPsi = cosf(yaw); + float sinPsi = sinf(yaw); + + dcm[0][0] = cosThe * cosPsi; + dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm[1][0] = cosThe * sinPsi; + dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm[2][0] = -sinThe; + dcm[2][1] = sinPhi * cosThe; + dcm[2][2] = cosPhi * cosThe; +} + +#endif diff --git a/flightcode/mavlink/c_library/mavlink_helpers.h b/flightcode/mavlink/c_library/mavlink_helpers.h new file mode 100644 index 0000000..0fa87fc --- /dev/null +++ b/flightcode/mavlink/c_library/mavlink_helpers.h @@ -0,0 +1,676 @@ +#ifndef _MAVLINK_HELPERS_H_ +#define _MAVLINK_HELPERS_H_ + +#include "string.h" +#include "checksum.h" +#include "mavlink_types.h" +#include "mavlink_conversions.h" + +#ifndef MAVLINK_HELPER +#define MAVLINK_HELPER +#endif + +/* + * Internal function to give access to the channel status for each channel + */ +#ifndef MAVLINK_GET_CHANNEL_STATUS +MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ +#ifdef MAVLINK_EXTERNAL_RX_STATUS + // No m_mavlink_status array defined in function, + // has to be defined externally +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#endif + return &m_mavlink_status[chan]; +} +#endif + +/* + * Internal function to give access to the channel buffer for each channel + */ +#ifndef MAVLINK_GET_CHANNEL_BUFFER +MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +{ + +#ifdef MAVLINK_EXTERNAL_RX_BUFFER + // No m_mavlink_buffer array defined in function, + // has to be defined externally +#else + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; +#endif + return &m_mavlink_buffer[chan]; +} +#endif + +/** + * @brief Reset the status of a channel. + */ +MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length + */ +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length, uint8_t crc_extra) +#else +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length) +#endif +{ + // This code part is the same for all messages; + msg->magic = MAVLINK_STX; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per channel + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; + msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &msg->checksum); +#endif + mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); + + return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + */ +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t length, uint8_t crc_extra) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); +} +#else +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t length) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); +} +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); + +/** + * @brief Finalize a MAVLink message with channel assignment and send + */ +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra) +#else +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) +#endif +{ + uint16_t checksum; + uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; + uint8_t ck[2]; + mavlink_status_t *status = mavlink_get_channel_status(chan); + buf[0] = MAVLINK_STX; + buf[1] = length; + buf[2] = status->current_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + status->current_tx_seq++; + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&checksum, packet, length); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + ck[0] = (uint8_t)(checksum & 0xFF); + ck[1] = (uint8_t)(checksum >> 8); + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); + _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, packet, length); + _mavlink_send_uart(chan, (const char *)ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); +} + +/** + * @brief re-send a message over a uart channel + * this is more stack efficient than re-marshalling the message + */ +MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) +{ + uint8_t ck[2]; + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); + _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); + _mavlink_send_uart(chan, (const char *)ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a message to send it over a serial byte stream + */ +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) +{ + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; +} + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->checksum); +} + +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->checksum); +} + +/** + * This is a varient of mavlink_frame_char() but with caller supplied + * parsing buffers. It is useful when you want to create a MAVLink + * parser in a library that doesn't use any global variables + * + * @param rxmsg parsing message buffer + * @param status parsing starus buffer + * @param c The char to parse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @param returnStats if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * + * A typical use scenario of this function call is: + * + * @code + * #include + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, + mavlink_status_t* status, + uint8_t c, + mavlink_message_t* r_message, + mavlink_status_t* r_mavlink_status) +{ + /* + default message crc function. You can override this per-system to + put this data in a different memory segment + */ +#if MAVLINK_CRC_EXTRA +#ifndef MAVLINK_MESSAGE_CRC + static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] +#endif +#endif + + /* Enable this option to check the length of each message. + This allows invalid messages to be caught much sooner. Use if the transmission + medium is prone to missing (or extra) characters (e.g. a radio that fades in + and out). Only use if the channel will only contain messages types listed in + the headers. + */ +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH +#ifndef MAVLINK_MESSAGE_LENGTH + static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] +#endif +#endif + + int bufferIndex = 0; + + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + rxmsg->magic = c; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received +/* Support shorter buffers than the + default maximum packet size */ +#if (MAVLINK_MAX_PAYLOAD_LEN < 255) + || c > MAVLINK_MAX_PAYLOAD_LEN +#endif + ) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) + { + status->parse_error++; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: +#if MAVLINK_CRC_EXTRA + mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); +#endif + if (c != (rxmsg->checksum & 0xFF)) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: + if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) { + // got a bad CRC message + status->msg_received = MAVLINK_FRAMING_BAD_CRC; + } else { + // Successfully got message + status->msg_received = MAVLINK_FRAMING_OK; + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == MAVLINK_FRAMING_OK) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg + r_mavlink_status->parse_state = status->parse_state; + r_mavlink_status->packet_idx = status->packet_idx; + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + + if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { + /* + the CRC came out wrong. We now need to overwrite the + msg CRC with the one on the wire so that if the + caller decides to forward the message anyway that + mavlink_msg_to_send_buffer() won't overwrite the + checksum + */ + r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8); + } + + return status->msg_received; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. This function will return 0, 1 or + * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC) + * + * Messages are parsed into an internal buffer (one for each channel). When a complete + * message is received it is copies into *returnMsg and the channel's status is + * copied into *returnStats. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to parse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @param returnStats if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * + * A typical use scenario of this function call is: + * + * @code + * #include + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan), + mavlink_get_channel_status(chan), + c, + r_message, + r_mavlink_status); +} + + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. This function will return 0 or 1. + * + * Messages are parsed into an internal buffer (one for each channel). When a complete + * message is received it is copies into *returnMsg and the channel's status is + * copied into *returnStats. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to parse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @param returnStats if a message was decoded, this is filled with the channel's stats + * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC + * + * A typical use scenario of this function call is: + * + * @code + * #include + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); + if (msg_received == MAVLINK_FRAMING_BAD_CRC) { + // we got a bad CRC. Treat as a parse failure + mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); + mavlink_status_t* status = mavlink_get_channel_status(chan); + status->parse_error++; + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + return 0; + } + return msg_received; +} + +/** + * @brief Put a bitfield of length 1-32 bit into the buffer + * + * @param b the value to add, will be encoded in the bitfield + * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. + * @param packet_index the position in the packet (the index of the first byte to use) + * @param bit_index the position in the byte (the index of the first bit to use) + * @param buffer packet buffer to write into + * @return new position of the last used byte in the buffer + */ +MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +{ + uint16_t bits_remain = bits; + // Transform number into network order + int32_t v; + uint8_t i_bit_index, i_byte_index, curr_bits_n; +#if MAVLINK_NEED_BYTE_SWAP + union { + int32_t i; + uint8_t b[4]; + } bin, bout; + bin.i = b; + bout.b[0] = bin.b[3]; + bout.b[1] = bin.b[2]; + bout.b[2] = bin.b[1]; + bout.b[3] = bin.b[0]; + v = bout.i; +#else + v = b; +#endif + + // buffer in + // 01100000 01000000 00000000 11110001 + // buffer out + // 11110001 00000000 01000000 01100000 + + // Existing partly filled byte (four free slots) + // 0111xxxx + + // Mask n free bits + // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 + // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 + + // Shift n bits into the right position + // out = in >> n; + + // Mask and shift bytes + i_bit_index = bit_index; + i_byte_index = packet_index; + if (bit_index > 0) + { + // If bits were available at start, they were available + // in the byte before the current index + i_byte_index--; + } + + // While bits have not been packed yet + while (bits_remain > 0) + { + // Bits still have to be packed + // there can be more than 8 bits, so + // we might have to pack them into more than one byte + + // First pack everything we can into the current 'open' byte + //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + //FIXME + if (bits_remain <= (uint8_t)(8 - i_bit_index)) + { + // Enough space + curr_bits_n = (uint8_t)bits_remain; + } + else + { + curr_bits_n = (8 - i_bit_index); + } + + // Pack these n bits into the current byte + // Mask out whatever was at that position with ones (xxx11111) + buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); + // Put content to this position, by masking out the non-used part + buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); + + // Increment the bit index + i_bit_index += curr_bits_n; + + // Now proceed to the next byte, if necessary + bits_remain -= curr_bits_n; + if (bits_remain > 0) + { + // Offer another 8 bits / one byte + i_byte_index++; + i_bit_index = 0; + } + } + + *r_bit_index = i_bit_index; + // If a partly filled byte is present, mark this as consumed + if (i_bit_index != 7) i_byte_index++; + return i_byte_index - packet_index; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define comm_send_ch() if you wish +// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a +// whole packet at a time + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + */ + +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) +{ +#ifdef MAVLINK_SEND_UART_BYTES + /* this is the more efficient approach, if the platform + defines it */ + MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len); +#else + /* fallback to one byte at a time */ + uint16_t i; + for (i = 0; i < len; i++) { + comm_send_ch(chan, (uint8_t)buf[i]); + } +#endif +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/flightcode/mavlink/c_library/mavlink_types.h b/flightcode/mavlink/c_library/mavlink_types.h new file mode 100644 index 0000000..0a98ccc --- /dev/null +++ b/flightcode/mavlink/c_library/mavlink_types.h @@ -0,0 +1,228 @@ +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +// Visual Studio versions before 2010 don't have stdint.h, so we just error out. +#if (defined _MSC_VER) && (_MSC_VER < 1600) +#error "The C-MAVLink implementation requires Visual Studio 2010 or greater" +#endif + +#include + +// Macro to define packed structures +#ifdef __GNUC__ + #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed)) +#else + #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) +#endif + +#ifndef MAVLINK_MAX_PAYLOAD_LEN +// it is possible to override this, but be careful! +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length +#endif + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length + +#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 +#define MAVLINK_EXTENDED_HEADER_LEN 14 + +#if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__) + /* full fledged 32bit++ OS */ + #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 +#else + /* small microcontrollers */ + #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 +#endif + +#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) + + +/** + * Old-style 4 byte param union + * + * This struct is the data format to be used when sending + * parameters. The parameter should be copied to the native + * type (without type conversion) + * and re-instanted on the receiving side using the + * native type as well. + */ +MAVPACKED( +typedef struct param_union { + union { + float param_float; + int32_t param_int32; + uint32_t param_uint32; + int16_t param_int16; + uint16_t param_uint16; + int8_t param_int8; + uint8_t param_uint8; + uint8_t bytes[4]; + }; + uint8_t type; +}) mavlink_param_union_t; + + +/** + * New-style 8 byte param union + * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering. + * The mavlink_param_union_double_t will be treated as a little-endian structure. + * + * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero. + * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the + * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. + * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, + * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86, + * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, + * and the bits pulled out using the shifts/masks. +*/ +MAVPACKED( +typedef struct param_union_extended { + union { + struct { + uint8_t is_double:1; + uint8_t mavlink_type:7; + union { + char c; + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; + float f; + uint8_t align[7]; + }; + }; + uint8_t data[8]; + }; +}) mavlink_param_union_double_t; + +/** + * This structure is required to make the mavlink_send_xxx convenience functions + * work, as it tells the library what the current system and component ID are. + */ +MAVPACKED( +typedef struct __mavlink_system { + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function +}) mavlink_system_t; + +MAVPACKED( +typedef struct __mavlink_message { + uint16_t checksum; ///< sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; +}) mavlink_message_t; + +MAVPACKED( +typedef struct __mavlink_extended_message { + mavlink_message_t base_msg; + int32_t extended_payload_len; ///< Length of extended payload if any + uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; +}) mavlink_extended_message_t; + +typedef enum { + MAVLINK_TYPE_CHAR = 0, + MAVLINK_TYPE_UINT8_T = 1, + MAVLINK_TYPE_INT8_T = 2, + MAVLINK_TYPE_UINT16_T = 3, + MAVLINK_TYPE_INT16_T = 4, + MAVLINK_TYPE_UINT32_T = 5, + MAVLINK_TYPE_INT32_T = 6, + MAVLINK_TYPE_UINT64_T = 7, + MAVLINK_TYPE_INT64_T = 8, + MAVLINK_TYPE_FLOAT = 9, + MAVLINK_TYPE_DOUBLE = 10 +} mavlink_message_type_t; + +#define MAVLINK_MAX_FIELDS 64 + +typedef struct __mavlink_field_info { + const char *name; // name of this field + const char *print_format; // printing format hint, or NULL + mavlink_message_type_t type; // type of this field + unsigned int array_length; // if non-zero, field is an array + unsigned int wire_offset; // offset of each field in the payload + unsigned int structure_offset; // offset in a C structure +} mavlink_field_info_t; + +// note that in this structure the order of fields is the order +// in the XML file, not necessary the wire order +typedef struct __mavlink_message_info { + const char *name; // name of the message + unsigned num_fields; // how many fields in this message + mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information +} mavlink_message_info_t; + +#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) +#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) + +// checksum is immediately after the payload bytes +#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) +#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) + +typedef enum { + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3 +} mavlink_channel_t; + +/* + * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number + * of buffers they will use. If more are used, then the result will be + * a stack overrun + */ +#ifndef MAVLINK_COMM_NUM_BUFFERS +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +# define MAVLINK_COMM_NUM_BUFFERS 16 +#else +# define MAVLINK_COMM_NUM_BUFFERS 4 +#endif +#endif + +typedef enum { + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1, + MAVLINK_PARSE_STATE_GOT_BAD_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef enum { + MAVLINK_FRAMING_INCOMPLETE=0, + MAVLINK_FRAMING_OK=1, + MAVLINK_FRAMING_BAD_CRC=2 +} mavlink_framing_t; + +typedef struct __mavlink_status { + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#define MAVLINK_BIG_ENDIAN 0 +#define MAVLINK_LITTLE_ENDIAN 1 + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/flightcode/mavlink/c_library/protocol.h b/flightcode/mavlink/c_library/protocol.h new file mode 100644 index 0000000..bf20708 --- /dev/null +++ b/flightcode/mavlink/c_library/protocol.h @@ -0,0 +1,339 @@ +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "string.h" +#include "mavlink_types.h" + +/* + If you want MAVLink on a system that is native big-endian, + you need to define NATIVE_BIG_ENDIAN +*/ +#ifdef NATIVE_BIG_ENDIAN +# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN) +#else +# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN) +#endif + +#ifndef MAVLINK_STACK_BUFFER +#define MAVLINK_STACK_BUFFER 0 +#endif + +#ifndef MAVLINK_AVOID_GCC_STACK_BUG +# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__) +#endif + +#ifndef MAVLINK_ASSERT +#define MAVLINK_ASSERT(x) +#endif + +#ifndef MAVLINK_START_UART_SEND +#define MAVLINK_START_UART_SEND(chan, length) +#endif + +#ifndef MAVLINK_END_UART_SEND +#define MAVLINK_END_UART_SEND(chan, length) +#endif + +/* option to provide alternative implementation of mavlink_helpers.h */ +#ifdef MAVLINK_SEPARATE_HELPERS + + #define MAVLINK_HELPER + + /* decls in sync with those in mavlink_helpers.h */ + #ifndef MAVLINK_GET_CHANNEL_STATUS + MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); + #endif + MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan); + #if MAVLINK_CRC_EXTRA + MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length, uint8_t crc_extra); + MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t length, uint8_t crc_extra); + #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra); + #endif + #else + MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length); + MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t length); + #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); + #endif + #endif // MAVLINK_CRC_EXTRA + MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); + MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); + MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c); + MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, + mavlink_status_t* status, + uint8_t c, + mavlink_message_t* r_message, + mavlink_status_t* r_mavlink_status); + MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); + MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); + MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, + uint8_t* r_bit_index, uint8_t* buffer); + #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); + MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg); + #endif + +#else + + #define MAVLINK_HELPER static inline + #include "mavlink_helpers.h" + +#endif // MAVLINK_SEPARATE_HELPERS + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +#if MAVLINK_NEED_BYTE_SWAP +static inline void byte_swap_2(char *dst, const char *src) +{ + dst[0] = src[1]; + dst[1] = src[0]; +} +static inline void byte_swap_4(char *dst, const char *src) +{ + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; +} +static inline void byte_swap_8(char *dst, const char *src) +{ + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; +} +#elif !MAVLINK_ALIGNED_FIELDS +static inline void byte_copy_2(char *dst, const char *src) +{ + dst[0] = src[0]; + dst[1] = src[1]; +} +static inline void byte_copy_4(char *dst, const char *src) +{ + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; +} +static inline void byte_copy_8(char *dst, const char *src) +{ + memcpy(dst, src, 8); +} +#endif + +#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b +#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b +#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b + +#if MAVLINK_NEED_BYTE_SWAP +#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#elif !MAVLINK_ALIGNED_FIELDS +#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#else +#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b +#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b +#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b +#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b +#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b +#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b +#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b +#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b +#endif + +/* + like memcpy(), but if src is NULL, do a memset to zero +*/ +static inline void mav_array_memcpy(void *dest, const void *src, size_t n) +{ + if (src == NULL) { + memset(dest, 0, n); + } else { + memcpy(dest, src, n); + } +} + +/* + * Place a char array into a buffer + */ +static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) +{ + mav_array_memcpy(&buf[wire_offset], b, array_length); + +} + +/* + * Place a uint8_t array into a buffer + */ +static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +{ + mav_array_memcpy(&buf[wire_offset], b, array_length); + +} + +/* + * Place a int8_t array into a buffer + */ +static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) +{ + mav_array_memcpy(&buf[wire_offset], b, array_length); + +} + +#if MAVLINK_NEED_BYTE_SWAP +#define _MAV_PUT_ARRAY(TYPE, V) \ +static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +{ \ + if (b == NULL) { \ + memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ + } else { \ + uint16_t i; \ + for (i=0; i" + exit 1 +} + +if [ -z "${1}" ]; then + echo "argument expected" + usage +fi + +if [ ! -d "${1}/message_definitions" ]; then + echo "${1}/message_definitions not found" + usage +fi + +outdir=`pwd`/c_library + +pushd ${1} + +python -m pymavlink.tools.mavgen --lang C -o ${outdir} \ + message_definitions/v1.0/ardupilotmega.xml + +if which git; then + git log --oneline > ${outdir}/.generate +else + echo "git not found" > ${outdir}/.generate +fi + +popd diff --git a/flightcode/mavpkt/Makefile b/flightcode/mavpkt/Makefile new file mode 100644 index 0000000..ea20161 --- /dev/null +++ b/flightcode/mavpkt/Makefile @@ -0,0 +1,34 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../mavlink/c_library/ardupilotmega + +INCS = -I../util -I../mavlink/c_library/ardupilotmega + +CFLAGS += -Wall -O2 $(INCS) +CXXFLAGS += -Wall -O2 $(INCS) + +SRCS_CPP = mavpkt.cpp +SRCS_C = util.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = mavpkt + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +.PHONY: clean diff --git a/flightcode/mavpkt/mavpkt.cpp b/flightcode/mavpkt/mavpkt.cpp new file mode 100644 index 0000000..ca39a24 --- /dev/null +++ b/flightcode/mavpkt/mavpkt.cpp @@ -0,0 +1,712 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink.h" + +#include "util.h" + +#if (defined __MACH__ && defined __APPLE__) +#define B460800 460800 +#define B921600 921600 +#endif + +// mavpkt +// +// Receive byte stream and output complete mavlink packets +// +// Input can be any file descriptor (i.e. stdin, file, serial port). +// Output is normally a datagram-ish file descriptor. + +static void usage(void) +{ + fprintf(stderr, "mavpkt\n"); + fprintf(stderr, " input defaults to stdin\n"); + fprintf(stderr, " -i tlog input is tlog format (default no)\n"); + fprintf(stderr, " -i udp://0.0.0.0:7007 input from the specified UDP port\n"); + fprintf(stderr, " output defaults to stdout\n"); + fprintf(stderr, " -o tlog output is tlog format (default no)\n"); + fprintf(stderr, " -o udp://127.0.0.1:6006 output to the specified UDP port\n"); + fprintf(stderr, " -r realtime (requires tlog input)\n"); + fprintf(stderr, " -s print stats to stderr at end\n"); + fprintf(stderr, " -v print input packets to stderr\n"); + exit(1); +} + +// 'tlog' format means each mavlink packet is preceeded by the time since the +// epoch in microseconds as a 64-bit big-endian unsigned. + +// 'realtime' means packets will be emitted in approximately real time, +// according to the timestamps in the input (which must be tlog). Allows +// "playing back" a tlog. + +// -i udp://0.0.0.0:7007 means mavpkt will read packets from this UDP port. +// This example means the read socket will be bound to INADDR_ANY, port 7007, +// so an external program should send packets to that port on this machine. + +// -o udp://127.0.0.1:6006 means mavpkt will send to this UDP port. This +// example means an ouput socket will be created and bound to a system- +// supplied port, and will send packets to localhost, port 6006. + +// -i serial:///dev/ttymxc1?baud=115200,flow=1 reads from the given serial +// port with a specified baud rate. flow=1 enables hardware flow control. + +// Examples: +// +// Read raw packets from a port and write a tlog: +// mavpkt -i udp://0.0.0.0:9000 -o tlog > packets.tlog +// +// Read raw packets from a port and write a raw data file: +// mavpkt -i udp://0.0.0.0:9000 > packets.bin +// +// Play a tlog to UDP port 8800 in real time: +// mavpkt -i tlog -o udp://127.0.0.1:8800 -r < packets.tlog +// +// Show stats for a tlog file: +// mavpkt -i tlog -s < packets.tlog > /dev/null + + +// MAVLINK_MESSAGE_INFO has 256 elements; sizeof(mavlink_info)=528384 +static mavlink_message_info_t mavlink_info[] = MAVLINK_MESSAGE_INFO; + +// MAVLINK_MESSAGE_CRCS is an array of 256 bytes +static uint8_t mavlink_crc[] = MAVLINK_MESSAGE_CRCS; + +// A mavlink packet is limited to 6+255+2 = 263 bytes +// 6 byte header, 255 max bytes in payload, 2 byte crc + +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +// input stream bytes to read at once +#define READ_MAX 1024 + +static uint8_t data_in[READ_MAX]; + + +// unpack_uint64 - unpack uint64_t from byte stream +static uint64_t unpack_uint64(const uint8_t* p) +{ + uint64_t d = 0; + for (int i = 0; i < 8; i++) + d = (d << 8) | *p++; + return d; +} + + +// pack_uint64 - pack uint64_t into byte stream +static void pack_uint64(uint64_t d, uint8_t* p) +{ + for (int i = 56; i >= 0; i -= 8) + *p++ = d >> i; +} + + +// calc_crc - calculate the CRC of a mavlink message +// +// CRC is seeded, then starts with the byte after the magic, continues through +// the payload up to the CRC in the message, then add the message-id-specific +// byte. +// +// Returns the 16-bit CRC. +static uint16_t calc_crc(const uint8_t* p) +{ + uint8_t payload_len = p[1]; + uint8_t msg_id = p[5]; + unsigned crc_len = 5 + payload_len; + + uint16_t crc = crc_calculate(p + 1, crc_len); + + crc_accumulate(mavlink_crc[msg_id], &crc); + + return crc; + +} // calc_crc + + +// get_crc - get (extract) the CRC of a mavlink message +// +// Returns the 16-bit CRC. +static uint16_t get_crc(const uint8_t* p) +{ + uint8_t payload_len = p[1]; + + uint16_t crc = p[6 + payload_len + 1]; // msb + crc <<= 8; + crc |= p[6 + payload_len]; // lsb + + return crc; + +} // get_crc + + +// check_crc - check the crc of a mavlink message +// +// Returns true if CRC correct, else false. +static bool check_crc(const uint8_t* p) +{ + return calc_crc(p) == get_crc(p); +} + + +// When accumulating statistics, we keep track of messages received from each +// (system, comeonent). Rather than allowing for all combinations (65536 of +// them) we just keep track of the first SOURCES_MAX we see. If there are more +// than that, the stats will be wrong. Dynamic allocation might be better, but +// really if there are more than a handful, the input data is probably junk. +#define SOURCES_MAX 10 + +static struct +{ + unsigned total; + unsigned crc_err; + unsigned bytes_discarded; + uint64_t first_us; + uint64_t last_us; + unsigned num_sources; + struct + { + unsigned total; + unsigned crc_err; + } by_id[256]; + struct + { + uint8_t in_use; + uint8_t sys; + uint8_t comp; + unsigned total; + unsigned crc_err; + struct + { + unsigned total; + unsigned crc_err; + } by_id[256]; + } by_src[SOURCES_MAX]; +} pkt_stats; + + +static void pkt_stats_init(void) +{ + memset(&pkt_stats, 0, sizeof(pkt_stats)); +} + + +// pkt_stats_add - add packet to statistics +static void pkt_stats_add(uint8_t* p) +{ + // 0..7 timestamp + // 8 magic + // 9 length + // 10 sequence + + uint64_t pkt_us = unpack_uint64(p); + uint8_t sys = p[11]; + uint8_t comp = p[12]; + uint8_t id = p[13]; + + // 1 if crc bad, 0 if crc good + unsigned crc_err = check_crc(p + 8) ? 0 : 1; + + pkt_stats.total++; + pkt_stats.crc_err += crc_err; + pkt_stats.by_id[id].total++; + pkt_stats.by_id[id].crc_err += crc_err; + + if (pkt_stats.first_us == 0) + pkt_stats.first_us = pkt_us; + pkt_stats.last_us = pkt_us; + + for (int s = 0; s < SOURCES_MAX; s++) + { + if (!pkt_stats.by_src[s].in_use) + { + pkt_stats.by_src[s].in_use = true; + pkt_stats.by_src[s].sys = sys; + pkt_stats.by_src[s].comp = comp; + pkt_stats.num_sources++; + } + if (pkt_stats.by_src[s].sys == sys && + pkt_stats.by_src[s].comp == comp) + { + pkt_stats.by_src[s].total++; + pkt_stats.by_src[s].crc_err += crc_err; + pkt_stats.by_src[s].by_id[id].total++; + pkt_stats.by_src[s].by_id[id].crc_err += crc_err; + return; + } + } + pkt_stats.num_sources++; +} // pkt_stats_add + + +// pkt_stats_print - print accumulated statistics to stderr +static void pkt_stats_print(void) +{ + + fprintf(stderr, "\n"); + fprintf(stderr, "packets: %u\n", pkt_stats.total); + fprintf(stderr, "crc errors: %u\n", pkt_stats.crc_err); + fprintf(stderr, "bytes discarded: %u\n", pkt_stats.bytes_discarded); + char ts_buf[32]; + fprintf(stderr, "first packet: %s\n", clock_tostr_r(pkt_stats.first_us, ts_buf)); + fprintf(stderr, "last packet: %s\n", clock_tostr_r(pkt_stats.last_us, ts_buf)); + fprintf(stderr, "sources: %u\n", pkt_stats.num_sources); + uint64_t span_us = pkt_stats.last_us - pkt_stats.first_us; + uint64_t us_per_s = 1000000; + uint64_t us_per_m = 60 * us_per_s; + uint64_t us_per_h = 60 * us_per_m; + unsigned span_h = span_us / us_per_h; + span_us -= (span_h * us_per_h); + unsigned span_m = span_us / us_per_m; + span_us -= (span_m * us_per_m); + unsigned span_s = span_us / us_per_s; + span_us -= (span_s * us_per_s); + unsigned span_ms = span_us / 1000; + fprintf(stderr, "time span: %02u:%02u:%02u.%03u\n", span_h, span_m, span_s, span_ms); + fprintf(stderr, "\n"); + // table heading, e.g. + // " id 0,0 1,1 10,0 1,154 total" + fprintf(stderr, " id"); + for (int s = 0; s < SOURCES_MAX && pkt_stats.by_src[s].in_use; s++) + fprintf(stderr, " %5u,%-5u", pkt_stats.by_src[s].sys, pkt_stats.by_src[s].comp); + fprintf(stderr, " total\n"); + // print row for each id we got any packets for + for (int id = 0; id < 256; id++) + { + if (pkt_stats.by_id[id].total == 0) + continue; + // table row, e.g. + // " 0 -/- 22/0 -/- -/- 22/0 HEARTBEAT" + fprintf(stderr, "%3d", id); + for (int s = 0; s < SOURCES_MAX && pkt_stats.by_src[s].in_use; s++) + { + if (pkt_stats.by_src[s].by_id[id].total > 0) + fprintf(stderr, " %5u/%-5u", pkt_stats.by_src[s].by_id[id].total, + pkt_stats.by_src[s].by_id[id].crc_err); + else + fprintf(stderr, " %5s/%-5s", "-", "-"); + } + fprintf(stderr, " %5d/%-5d", pkt_stats.by_id[id].total, pkt_stats.by_id[id].crc_err); + fprintf(stderr, " %-s\n", mavlink_info[id].name); + } + +} // pkt_stats_print + + +// udp_uri - parse uri for udp address and create socket for it +// +// is_dest determines whether the parsed address is where we send packets +// (is_dest true), or it is our local address, which we will bind to and read +// packets from (is_dest false). +// +// is_dest true: create a socket and connect it to the parsed address +// is_dest false: create a socket and bind it to the parsed address +// +// Note that for UDP sockets, 'connect' to an address saves the address as the +// destination to use in future send() calls. +// +// Returns: fd of socket on success, or -1 on error +static int udp_uri(const char* uri, bool is_dest) +{ + + if (strncmp(uri, "udp:", 4) != 0) + return -1; + + unsigned int ip1, ip2, ip3, ip4, port; + if (sscanf(uri, "udp://%u.%u.%u.%u:%u", &ip1, &ip2, &ip3, &ip4, &port) != 5 || + ip1 > 255 || ip2 > 255 || ip3 > 255 || ip4 > 255 || port > 65535) + { + fprintf(stderr, "error parsing \"%s\"\n", uri); + return -1; + } + + uint32_t ip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | ip4; + struct sockaddr_in sa_uri; + memset(&sa_uri, 0, sizeof(sa_uri)); + sa_uri.sin_family = AF_INET; + sa_uri.sin_addr.s_addr = htonl(ip); + sa_uri.sin_port = htons(port); + + struct sockaddr_in sa_any; + memset(&sa_any, 0, sizeof(sa_any)); + sa_any.sin_family = AF_INET; + sa_any.sin_addr.s_addr = htonl(INADDR_ANY); + sa_any.sin_port = htons(0); + + int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) + { + perror("socket"); + return -1; + } + + // is_dest = false means the uri is the address of this socket + // is_dest = true means the uri is where we send packets + + if (is_dest) + { + if (bind(fd, (struct sockaddr*)&sa_any, sizeof(sa_any)) != 0) + { + perror("bind"); + close(fd); + return -1; + } + // connect sets the default destination address + if (connect(fd, (struct sockaddr*)&sa_uri, sizeof(sa_uri)) != 0) + { + perror("connect"); + close(fd); + return -1; + } + } + else + { + // just bind to specified uri + if (bind(fd, (struct sockaddr*)&sa_uri, sizeof(sa_uri)) != 0) + { + perror("bind"); + close(fd); + return -1; + } + } + + return fd; + +} // udp_uri + + +// serial_uri - parse uri for serial port and create fd for it +// +// Returns: fd of port on success, or -1 on error +static int serial_uri(const char* uri) +{ + + if (strncmp(uri, "serial:", 7) != 0) + return -1; + + char dev_opts[80]; + if (sscanf(uri, "serial://%s", dev_opts) != 1) + { + fprintf(stderr, "error parsing \"%s\"\n", uri); + return -1; + } + + // /dev/ttyS0?baud=115200,flow=1 + char *str = dev_opts; + char *dev_name = strsep(&str, "?"); + unsigned int baud = 0; + unsigned int flow = 0; // 0=none, 1=hw (2=sw TBD) + while (str != NULL && *str != '\0') + { + char *opt = strsep(&str, ","); + if (sscanf(opt, "baud=%u", &baud) == 1) + continue; + else if (sscanf(opt, "flow=%u", &flow) == 1) + continue; + else + { + fprintf(stderr, "error parsing \"%s\"\n", uri); + return -1; + } + } + + int fd = open(dev_name, O_RDWR | O_NOCTTY); + if (fd < 0) + { + perror("open"); + return -1; + } + + struct termios options; + + memset(&options, 0, sizeof(options)); + + tcgetattr(fd, &options); + + switch (baud) + { + case 57600: cfsetspeed(&options, B57600); break; + case 115200: cfsetspeed(&options, B115200); break; + case 230400: cfsetspeed(&options, B230400); break; + case 460800: cfsetspeed(&options, B460800); break; + case 921600: cfsetspeed(&options, B921600); break; + default: + fprintf(stderr, "unsupported baudrate %u", baud); + return -1; + } + + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; + options.c_cc[VMIN] = 20; + options.c_cc[VTIME] = 0; + + if (flow == 1) + { + options.c_cflag |= CRTSCTS; + } + else + { + options.c_cflag &= ~CRTSCTS; + options.c_cflag |= CLOCAL; + } + + tcflush(fd, TCIFLUSH); + + tcsetattr(fd, TCSANOW, &options); + + return fd; + +} // serial_uri + + +// print_pkt - print packet to stderr +static void print_pkt(const uint8_t* p, bool tlog) +{ + + uint64_t ts = unpack_uint64(p); + char ts_buf[32]; + fprintf(stderr, "%s ", clock_tostr_r(ts, ts_buf)); + + p += 8; + unsigned num_bytes = 6 + p[1] + 2; + for (unsigned i = 0; i < (num_bytes - 1); i++) + fprintf(stderr, "%02x ", *p++); + fprintf(stderr, "%02x\n", *p); + +} // print_pkt + + +// emit_packet - emit packet to file descriptor +// +// Packet can be emitted raw (just a write) or it can be emitted to a tlog +// (preceeded by timestamp). +// +// pkt supplied always has 8 bytes at the start for the timestamp. If not +// tlog, they are simply skipped. +static int emit_packet(int fd, const uint8_t* pkt, bool tlog, bool verbose) +{ + if (verbose) + print_pkt(pkt, tlog); + + int bytes; + if (tlog) + bytes = 8 + 6 + pkt[9] + 2; + else + { + bytes = 6 + pkt[9] + 2; + pkt += 8; + } + + if (write(fd, pkt, bytes) != bytes) + return -1; + + return bytes; + +} // emit_packet + + +int main(int argc, char* argv[]) +{ + int fd; + int fd_in = STDIN_FILENO; + int fd_out = STDOUT_FILENO; + bool tlog_in = false; + bool tlog_out = false; + unsigned data_in_bytes; + bool verbose = false; + bool realtime = false; + bool stats = false; + uint64_t rt_first_us = 0; // real time first packet emitted + int opt; + + //fprintf(stderr, "sizeof(mavlink_info) = %ld\n", sizeof(mavlink_info)); + //fprintf(stderr, "nelems(mavlink_info) = %ld\n", sizeof(mavlink_info)/sizeof(mavlink_info[0])); + + // + // Parse arguments + // + + while ((opt = getopt(argc, argv, "hi:o:rsv")) != -1) + { + switch (opt) + { + case 'h': + usage(); + break; + case 'i': + if (strcmp(optarg, "tlog") == 0) + tlog_in = true; + else if ((fd = udp_uri(optarg, false)) != -1) + fd_in = fd; + else if ((fd = serial_uri(optarg)) != -1) + fd_in = fd; + else + usage(); + break; + case 'o': + if (strcmp(optarg, "tlog") == 0) + tlog_out = true; + else if ((fd = udp_uri(optarg, true)) != -1) + fd_out = fd; + else + usage(); + break; + case 'r': + realtime = true; + break; + case 's': + stats = true; + break; + case 'v': + verbose = true; + break; + default: + usage(); + } + } + + // + // Initialize data structures + // + + pkt_stats_init(); + + memset(data_in, 0, sizeof(data_in)); + data_in_bytes = 0; + + // If the input data does not have a timestamp, leave space at the start + // to insert a timestamp in the output packet + if (!tlog_in) + data_in_bytes = 8; + + // + // Process input bytes, writing output packets + // + + while (1) + { + + unsigned read_max = READ_MAX - data_in_bytes; + int num_read = read(fd_in, data_in + data_in_bytes, read_max); + + if (num_read == 0) + { + // eof + break; + // XXX Needs fixing for serial input + } + + if (num_read < 0) + { + // error + perror("read"); + break; + } + + data_in_bytes += num_read; + + // Outer loop is in case we have more than one packet + bool emitted; + do + { + emitted = false; + + // Throw away data from the start until there's magic in the right + // place. This loop should almost never have to execute. + while (data_in_bytes > 8 && data_in[8] != MAVLINK_STX) + { + memmove(data_in, data_in + 1, --data_in_bytes); + pkt_stats.bytes_discarded++; + } + + // emit packet if we have one + if (data_in_bytes > 9 && data_in_bytes >= (unsigned)(8 + 6 + data_in[9] + 2)) + { + uint64_t now_us = clock_gettime_us(CLOCK_REALTIME); + + uint64_t ts_pkt_us; // timestamp of this packet + if (tlog_in) + { + // get timestamp from packet + ts_pkt_us = unpack_uint64(data_in); + } + else + { + // use timestamp from clock; save it in packet + ts_pkt_us = now_us; + pack_uint64(ts_pkt_us, data_in); + } + + pkt_stats_add(data_in); + + // if simulating realtime, delay until it is time for this packet to go + if (tlog_in && realtime) + { + if (rt_first_us == 0) + { + // real time first packet emitted + rt_first_us = now_us; + } + else + { + // delay until time for this packet to go + int64_t delay_us = (ts_pkt_us - pkt_stats.first_us) - (now_us - rt_first_us); + if (delay_us > 0) + usleep(delay_us); + } + } + + emit_packet(fd_out, data_in, tlog_out, verbose); + + // If the input is not tlog, we still leave space at front for + // timestamp, making it so we can stuff in the timestamp and + // write the output without needing a separate output buffer. + if (tlog_in) + { + // consume timestamp; copy data to very start of buffer + unsigned consume_bytes = 8 + 6 + data_in[9] + 2; + data_in_bytes -= consume_bytes; + memmove(data_in, data_in + consume_bytes, data_in_bytes); + } + else + { + // don't consume timestamp; copy data to right after timestamp + unsigned consume_bytes = 6 + data_in[9] + 2; + data_in_bytes -= consume_bytes; + memmove(data_in + 8, data_in + 8 + consume_bytes, data_in_bytes); + } + // This flag makes it so we'll check the input buffer again, + // in case we got more than one packet in the last read + // (which is common). + emitted = true; + } + + } + while (emitted); + + } // while (1) + + // + // Print statistics if requested + // + + if (stats) + pkt_stats_print(); + + exit(0); + +} // main diff --git a/flightcode/old/app_streamer.py b/flightcode/old/app_streamer.py new file mode 100755 index 0000000..5df5868 --- /dev/null +++ b/flightcode/old/app_streamer.py @@ -0,0 +1,103 @@ +#!/usr/bin/python + +# This uses the above '#!' instead of '#!/usr/bin/env python' so that the +# busybox 'pidof' can find this process by name. + +import ConfigParser +import clock +import logging +import logging.config +import socket +import sys + +solo_conf = "/etc/sololink.conf" + +logging.config.fileConfig(solo_conf) +logger = logging.getLogger("vid") + +logger.info("starting") + +# Read packets from 127.0.0.1:5600, and forward them to another IP, same port. +# 127.0.0.1:5600 is where the SN decoder streams video. +# Other IP address is expected to be the App. + +def get_app_ip(): + try: + f = open(app_address_file) + ip = f.read() + ip = ip.strip() + f.close() + return ip + except IOError: + return None + +src_ip = "" +src_port = 5600 + +dst_ip = None +dst_port = 5600 + +app_address_file = "" + +config = ConfigParser.SafeConfigParser() + +# if the config file is not found, and empty list is returned and the +# "get" operations later fail +config.read(solo_conf) + +# read configuration items +try: + app_address_file = config.get("solo", "appAddressFile") +except: + logger.error("error reading config from %s", solo_conf) + sys.exit(1) + +packet_count = 0 +byte_count = 0 + +now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + +log_interval_s = 10 +log_interval_us = long(log_interval_s * 1000000) +log_time_us = now_us + log_interval_us + +app_time_us = now_us +app_interval_us = 1000000 + +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, 191) #Highest level VI +sock.bind((src_ip, src_port)) + +while True: + # forward a packet + pkt = sock.recv(4096) + + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + + # To reduce jitter, send it first, then check for changes. This means + # when the destination IP changes, we'll probably not send the first + # packet, and send one too many at the end. + + if dst_ip is not None: + sock.sendto(pkt, (dst_ip, dst_port)) + packet_count += 1 + byte_count += len(pkt) + if now_us > log_time_us: + bytes_sec = byte_count / log_interval_s + logger.info("%0.1f pkts/s, %d bytes/s, %d bits/s)", + float(packet_count) / float(log_interval_s), + bytes_sec, bytes_sec * 8) + packet_count = 0 + byte_count = 0 + log_time_us += log_interval_us + else: + # avoid a barrage of catchup messages when the app goes away then comes back + log_time_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + log_interval_us + + if now_us > app_time_us: + # Check for change in dst_ip. It might change from None to some IP + # address, from an IP address to None, or from one IP address to another. + app_ip = get_app_ip() + if dst_ip != app_ip: + dst_ip = app_ip + app_time_us = now_us + app_interval_us diff --git a/flightcode/old/logd.py b/flightcode/old/logd.py new file mode 100755 index 0000000..920f9e7 --- /dev/null +++ b/flightcode/old/logd.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python + +import os +import cPickle +import datetime +import socket +import struct +import sys + +from stm32_defs import * + +# Log record types. +TYPE_STM32_PACKET = 1 +TYPE_PIXIN_PACKET = 2 +TYPE_PIXOUT_PACKET = 3 + +PIPE_NAME_DEFAULT = "/var/run/logd" + +# log to LOG_DIR_0_DEFAULT if it exists, else LOG_DIR_1_DEFAULT +LOG_DIR_0_DEFAULT = "/log" +LOG_DIR_1_DEFAULT = "/var/local" + +debug = False + + +class Stm32Logger(object): + + def __init__(self, pipe_name=PIPE_NAME_DEFAULT): + self.log_sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self.local_name = "/tmp/logd-stm32" + self.pipe_name = pipe_name + # delete it if there's an old one still there + try: + os.remove(self.local_name) + except: + pass # wasn't there, okay + self.log_sock.bind(self.local_name) + + def uninit(self): + os.remove(self.local_name) + + def log_packet(self, pktTime, pkt): + data = (TYPE_STM32_PACKET, datetime.datetime.now(), pktTime, pkt) + pick = cPickle.dumps(data) + try: + self.log_sock.sendto(pick, self.pipe_name) + except: + pass # logger is dead + + +class PixInLogger(object): + + def __init__(self, pipe_name=PIPE_NAME_DEFAULT): + self.log_sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self.local_name = "/tmp/logd-pixin" + self.pipe_name = pipe_name + # delete it if there's an old one still there + try: + os.remove(self.local_name) + except: + pass # wasn't there, okay + self.log_sock.bind(self.local_name) + + def uninit(self): + os.remove(self.local_name) + + def log_packet(self, pkt): + data = (TYPE_PIXIN_PACKET, datetime.datetime.now(), pkt) + pick = cPickle.dumps(data) + try: + self.log_sock.sendto(pick, self.pipe_name) + except: + pass # logger is dead + + +class PixOutLogger(object): + + def __init__(self, pipe_name=PIPE_NAME_DEFAULT): + self.log_sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self.local_name = "/tmp/logd-pixout" + self.pipe_name = pipe_name + # delete it if there's an old one still there + try: + os.remove(self.local_name) + except: + pass # wasn't there, okay + self.log_sock.bind(self.local_name) + + def uninit(self): + os.remove(self.local_name) + + def log_packet(self, pkt): + data = (TYPE_PIXOUT_PACKET, datetime.datetime.now(), pkt) + pick = cPickle.dumps(data) + try: + self.log_sock.sendto(pick, self.pipe_name) + except: + pass # logger is dead + + +class Logger(object): + + # XXX get from config file + + # If we roll the log and have more than MAX_FILES, old files are deleted + # to get down to MAX_FILES. + MAX_FILES = 50 + + # If a log file gets to MAX_FILE_BYTES, the log is rolled. + MAX_FILE_BYTES = 10000000 + + # If the total number of bytes logged gets to MAX_BYTES, old files are + # deleted to get down to MAX_BYTES. + MAX_BYTES = 75000000 # 90% of a 100 MB partition (~85 MB usable) + + def __init__(self, pipe_name, file_root, file_ext): + self.total_bytes = 0 + self.epoch = datetime.datetime(1970, 1, 1) + self.pipe_name = pipe_name + self.file_root = file_root + self.file_ext = file_ext + # delete named socket if left over from a previous run + try: + os.remove(self.pipe_name) + except: + pass # did not exist, okay + # create named socket to receive data on + self.sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + try: + self.sock.bind(self.pipe_name) + except: + print "ERROR creating pipe %s" % (self.pipe_name,) + raise + # always start with a new log file + self.roll() + + def file_name(self, n): + return "%s.%03d.%s" % (self.file_root, n, self.file_ext) + + # RC packets as received from STM32 serial port on Artoo + def format_stm32(self, data_tuple): + # data_tuple is: + # integer typeId + # datetime logTime + # datetime pktTime + # string packet + + typeId = data_tuple[0] + logTime = data_tuple[1] + pktTime = data_tuple[2] + pkt = data_tuple[3] + + logTime = logTime - self.epoch # timedelta + pktTime = pktTime - self.epoch # timedelta + pktLen = len(pkt) + + if pktLen > 0: + pktId = ord(pkt[0]) + else: + pktId = -1 + + s1 = "%d,%d,%d,%d,%d,%d,%d" % (typeId, + logTime.days * 86400 + logTime.seconds, + logTime.microseconds, + pktTime.days * 86400 + pktTime.seconds, + pktTime.microseconds, + pktLen, + pktId) + + if pktId == 0: + s2 = ",%s\n" % (pkt[1:],) + elif pktId == PKT_ID_DSM: + if pktLen == 17: + s2 = ",%d,%d,%d,%d,%d,%d,%d,%d\n" % (ord(pkt[2]) * 256 + ord(pkt[1]), + ord(pkt[4]) * 256 + ord(pkt[3]), + ord(pkt[6]) * 256 + ord(pkt[5]), + ord(pkt[8]) * 256 + ord(pkt[7]), + ord(pkt[10]) * 256 + ord(pkt[9]), + ord(pkt[12]) * 256 + ord(pkt[11]), + ord(pkt[14]) * 256 + ord(pkt[13]), + ord(pkt[16]) * 256 + ord(pkt[15])) + else: + s2 = "\n" + elif pktId == PKT_ID_SYSINFO: + # 12 bytes unique_id + # 2 bytes hw_version + # string sw_version + if pktLen >= 13: + # unique_id + s2 = ",%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x" % \ + (ord(pkt[1]), ord(pkt[2]), ord(pkt[3]), ord(pkt[4]), + ord(pkt[5]), ord(pkt[6]), ord(pkt[7]), ord(pkt[8]), + ord(pkt[9]), ord(pkt[10]), ord(pkt[11]), ord(pkt[12])) + if pktLen >= 15: + # hw_version + s2 += ",%02x:%02x" % (ord(pkt[13]), ord(pkt[14])) + if pktLen >= 16: + # sw_version + s2 += ",%s" % ("".join(pkt[15:]),) + s2 += "\n" + else: + s2 = "\n" + else: + s2 = "\n" + + return s1 + s2 + + # RC packets as received from UDP port on Solo + def format_pixin(self, data_tuple): + # data_tuple is: + # integer typeId + # datetime logTime + # string packet + + typeId = data_tuple[0] + logTime = data_tuple[1] + pkt = data_tuple[2] + + logTime = logTime - self.epoch # timedelta + pktLen = len(pkt) + + if pktLen != 26: + s = "%d,%d,%d\n" % (typeId, + logTime.days * 86400 + logTime.seconds, + logTime.microseconds) + return s + + # packet is: + # pktTime.usec 32 bits + # pktTime.sec 32 bits + # sequence 16 bits + # channel 1 16 bits + # : + # channel 8 16 bits + (usec, sec, seq, ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8) = \ + struct.unpack("H", pkt[pktIdx:pktIdx+2]) + pktIdx += 2 + # channels + for chIdx in range(1, 8): + (chRaw,) = struct.unpack(">H", pkt[pktIdx:pktIdx+2]) + pktIdx += 2 + chNum = (chRaw >> 11) & 0xf + chVal = chRaw & 0x7ff + s += ",%d,%d" % (chNum, chVal) + + return s + "\n" + + def roll(self): + # If there is not a log file, do nothing. + # Otherwise, rename it to + # file_root.001.file_ext + # bumping all old ones up. We keep up to MAX_FILES. + # + # The end result is: + # file_root.000.file_ext current + # file_root.001.file_ext | + # file_root.002.file_ext v + # file_root.003.file_ext oldest + # where ".003" goes up to MAX_FILES-1. + + self.total_bytes = 0 + + try: + stat_info = os.stat(self.file_name(0)) + except: + return # "current" file not there, nothing to do + + # Rename files, starting from high numbers and working down. This takes + # care of the MAX_FILES limit (the highest-numbered file is just + # overwritten). + for file_num in range(self.MAX_FILES-1, 0, -1): + src = self.file_name(file_num-1) + try: + stat_info = os.stat(src) + except: + # file does not exist + continue + # file exists + self.total_bytes += stat_info.st_size + dst = self.file_name(file_num) + try: + os.rename(src, dst) + if debug: + print "rename %s to %s" % (src, dst) + except: + pass # error renaming file? + ### end for file_num + + def check_size(self): + """check total size of logs and delete log files as necessary""" + + # if current log file is over 10% of the total, do a roll + try: + stat_info = os.stat(self.file_name(0)) + except: + return # "current" file not there, nothing to do + + if stat_info.st_size >= self.MAX_FILE_BYTES: + self.roll() + + # delete files until the total number of bytes is less that the max + for file_num in range(self.MAX_FILES-1, 0, -1): + if self.total_bytes <= self.MAX_BYTES: + break + src = self.file_name(file_num) + try: + stat_info = os.stat(src) + if debug: + print "delete %s" % (src,) + os.remove(src) + self.total_bytes -= stat_info.st_size + except: + pass # does not exist + + def run(self): + while True: + data = self.sock.recv(1024) + # data is pickled python, consisting of an integer type indicator + # followed by type-specific data. + try: + data_tuple = cPickle.loads(data) + except: + pickle_error += 1 + continue + typeId = data_tuple[0] + if typeId == TYPE_STM32_PACKET: + s = self.format_stm32(data_tuple) + elif typeId == TYPE_PIXIN_PACKET: + s = self.format_pixin(data_tuple) + elif typeId == TYPE_PIXOUT_PACKET: + s = self.format_pixout(data_tuple) + else: + s = "%d" % (typeId,) + try: + logfile = open(self.file_name(0), "a") + except: + print "ERROR opening log file %s for writing" % (self.file_name(0),) + raise + logfile.write(s) + logfile.close() + self.total_bytes += len(s) + self.check_size() + + +def is_dir(path): + try: + s = os.stat(path) + except: + return False + if s.st_mode & os.O_DIRECTORY: + return True + else: + return False + + +if __name__ == "__main__": + from optparse import OptionParser + + parser = OptionParser("logd.py [options]") + + parser.add_option("--pipe", dest="pipe_name", type="string", default=None, + help="logging pipe name") + + parser.add_option("--file", dest="file_root", type="string", default=None, + help="log file name root") + + (opts, args) = parser.parse_args() + + pipe_name = PIPE_NAME_DEFAULT + if opts.pipe_name is not None: + pipe_name = opts.pipe_name + + # set default path + + if is_dir(LOG_DIR_0_DEFAULT): + log_dir = LOG_DIR_0_DEFAULT # preferred + else: + log_dir = LOG_DIR_1_DEFAULT # must exist + file_root = log_dir + "/3dr-logd" + + if opts.file_root is not None: + file_root = opts.file_root + + file_ext = "csv" + + logger = Logger(pipe_name, file_root, file_ext) + + os.nice(19) + + logger.run() diff --git a/flightcode/old/mavproxy_solo.py b/flightcode/old/mavproxy_solo.py new file mode 100644 index 0000000..ffdd6a9 --- /dev/null +++ b/flightcode/old/mavproxy_solo.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python +"""solo status insertion""" + +from pymavlink import mavutil +from MAVProxy.modules.lib import mp_module +import iw +import logging +import logging.config + +class SoloModule(mp_module.MPModule): + + + # times per second to trigger in idle task + POLL_FREQ = 1 + + + def __init__(self, mpstate): + super(SoloModule, self).__init__(mpstate, "solo", + "solo status insertion", + public = True) + logging.config.fileConfig("/etc/sololink.conf") + self._logger = logging.getLogger("wifi") + self._period = mavutil.periodic_event(SoloModule.POLL_FREQ) + # Pixhawk is sending messages with srcSystem=1, srcComponent=1 + # Use the same srcSystem but a different srcComponent so the sequence + # numbers make sense. + # XXX How do we determine when we are running on Artoo, and should use + # a different srcSystem? + # The --source-system command line argment does not change the source + # system for downstream messages. Maybe it does for upstream messages. + self._srcSystem = 1 + self._srcComponent = 2 + self._logger.info("MAVProxy solo module running") + + + def link_rssi(self, info): + # map link info to rssi value (0..255) + # Using signal strength... + if False: + # Signal is dBm, rougly ~ -100..-30 + # Arbitrary mapping: + # <= -100 -> 0 + # >= -50 -> 255 + val = info["signal"] + valMin = -100.0 + valMax = -50.0 + rssiMin = 0.0 + rssiMax = 255.0 + # Using txBitrate... + if True: + # Rate is Mbit/sec, 1..54 + # Arbitrary mapping: + # <1 -> 0 + # 1 -> 10 valMin -> rssiMin + # 54 -> 255 valMax -> rssiMax + val = info["txBitrate"] + valMin = 1.0 + valMax = 54.0 + rssiMin = 10.0 + rssiMax = 255.0 + if val is None: + rssi = 0 + elif val < valMin: + rssi = 0 + elif val >= valMax: + rssi = int(rssiMax) + else: + rssi = int(((val - valMin) / (valMax - valMin)) * (rssiMax - rssiMin) + rssiMin) + # rssi should already be in range + if rssi < 0: + rssi = 0 + elif rssi > 255: + rssi = 255 + return rssi + + + def idle_task(self): + # idle_task is called 100 times/second + if self._period.trigger(): + # trigger rate is POLL_FREQ times/second + + info = iw.iwLink("wlan0") + + # info contains: txBytes rxBytes ssid txBitrate rxPackets freq signal txPackets + # log some at "info" level and some at "debug" level + self._logger.info("ssid=%s freq=%s signal=%s txBitrate=%s", + info["ssid"], info["freq"], + info["signal"], info["txBitrate"]) + self._logger.debug("txPackets=%s rxPackets=%s txBytes=%s rxBytes=%s", + info["txPackets"], info["rxPackets"], + info["txBytes"], info["rxBytes"]) + + rssi = self.link_rssi(info) + + # Send status to master. If sending to Pixhawk, this does not get + # echoed back out in the outgoing telemetry stream. + for master in self.mpstate.mav_master: + master.mav.srcSystem = self._srcSystem + master.mav.srcComponent = self._srcComponent + try: + msg = master.mav.radio_status_encode(rssi=rssi, remrssi=0, + txbuf=0, noise=0, + remnoise=0, rxerrors=0, + fixed=0) + except Exception as excEncode: + print "solo: master encode exception", excEncode + else: + try: + master.mav.send(msg) + except Exception as excSend: + print "solo: master send exception:", excSend + + # Send status to all slaves. + for slave in self.mpstate.mav_outputs: + # srcSystem/srcComponent are properties of the 'mav' object. + # Messages from upstream that are simply routed through are not + # affected by this change. This is done here instead of in the + # constructor since the outputs may change at runtime. + slave.mav.srcSystem = self._srcSystem + slave.mav.srcComponent = self._srcComponent + try: + msg = slave.mav.radio_status_encode(rssi=rssi, remrssi=0, + txbuf=0, noise=0, + remnoise=0, rxerrors=0, + fixed=0) + except Exception as excEncode: + print "solo: slave encode exception", excEncode + else: + try: + slave.mav.send(msg) + except Exception as excSend: + print "solo: slave send exception:", excSend + + +def init(mpstate): + """initialize module""" + return SoloModule(mpstate) diff --git a/flightcode/old/pixrc.py b/flightcode/old/pixrc.py new file mode 100755 index 0000000..ec6d4cc --- /dev/null +++ b/flightcode/old/pixrc.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python + +import ConfigParser +import datetime +import errno +import logd +import logging +import logging.config +import optparse +import os +import serial +import simple_stats +import socket +import struct +import sys +import threading +import time + + +logging.config.fileConfig("/etc/sololink.conf") +logger = logging.getLogger("pixrc") + +logger.info("starting (20141114_1132)") + +config = ConfigParser.SafeConfigParser() +config.read("/etc/sololink.conf") + +rcDsmDev = config.get("solo", "rcDsmDev") +rcDsmBaud = config.getint("solo", "rcDsmBaud") + +# Only accept RC packets from one of these +rcSourceIps = config.get("solo", "rcSourceIps").split(",") + +# RC packets arrive on this port +rcDestPort = config.getint("solo", "rcDestPort") + +logger.info("accept RC packets from %s on port %d", + rcSourceIps.__str__(), rcDestPort) + +CHANNEL_BITS = 11 +CHANNEL_MASK = 0x7ff + +SOCKET_TIMEOUT = 0.4 +DSM_INTERVAL = 0.020 + +dsmBeat = 0 +rcBeat = 0 + +# Debug only; enable/disable sending packets to Pixhawk +DSM_SEND_ENABLE = True + +rcChansOut = None + +# What we send Pixhawk if we stop receiving RC packets +# PWM values: [ throttle, roll, pitch, yaw ] +rcFailsafeChans = [ 0, 1500, 1500, 1500 ] + +logger.info("RC failsafe packet is %s", rcFailsafeChans.__str__()) + +# RC packets are received as a timestamp, sequence number, then channel data. +# * timestamp is microseconds since some epoch (64 bits) +# * sequence simply increments each packet (16 bits) and can be used to detect +# missed or out of order packets. +# * channel data is a PWM value per channel (16 bits) +# +# All data is received little-endian. A typical packet is as follows (8 +# channels, all 1500): +# +# timestamp seq ch0 ch1 ch2 ch3 ch4 ch5 ch6 ch7 +# |----- 3966001072 ----| 3965 1500 1500 1500 1500 1500 1500 1500 1500 +# b0 5f 64 ec 00 00 00 00 7d 0f dc 05 dc 05 dc 05 dc 05 dc 05 dc 05 dc 05 dc 05 + +# DSM is sent in 16-byte chunks, each contain 8 2-byte words: +# +# Word Data +# 0 0x00AB (magic) +# 1..7 (chNum << 11) | (chData & 0x7ff) +# +# For more than 7 channels, another packet is sent, starting with the magic. +# 16 bytes are always sent; unused channel slots are filled with 0xffff. +# +# Each word is sent big-endian. A typical byte stream might be as follows +# (8 channels, all =750): +# +# magic --0-- --1-- --2-- --3-- --4-- --5-- --6-- +# 00 ab 02 ee 0a ee 12 ee 1a ee 22 ee 2a ee 32 ee +# +# magic --7-- --x-- --x-- --x-- --x-- --x-- --x-- +# 00 ab 3a ee ff ff ff ff ff ff ff ff ff ff ff ff +# +# Note that the arriving UDP packet is PWM values in microseconds. +# Those are converted to 11-bit channel values by shifting down one bit. + + +def rcUnpack(packedData): + """Unpack RC packet. + + Returns a tuple (timestamp, sequence, channels[]). + """ + # Length of received packet determines how many channels there + # are: packet is 8 + 2 + (2 * numChans) bytes + # Sanity-check received packet + dataLen = len(packedData) + if dataLen < 10 or (dataLen & 1) != 0: + logger.warn("rcUnpack: malformed packet received (length = %d)", dataLen) + return None + # require 4..14 channels + numChans = (dataLen - 10) / 2 + if numChans < 4 or numChans > 14: + logger.warn("rcUnpack: malformed packet received (%d channels)", numChans) + return None + timestamp, sequence = struct.unpack(" 0: + dsmPacket += struct.pack(">H", 171) + # pack 7 channels before needing another magic + for c in range(0, 7): + if channelsLeft > 0: + # channel value is 1000...2000 + # needs to be 174...1874 + value = channels[channelNum] # 1000...2000 + + value = value * 1700 / 1000 - 1526 + if(value < 0): + value = 0 + if(value > 2000): + value = 2000; + + chan = (channelNum << CHANNEL_BITS) | (value & CHANNEL_MASK) + dsmPacket += struct.pack(">H", chan) + channelsLeft -= 1 + channelNum += 1 + else: + dsmPacket += struct.pack(">H", 65535) + return dsmPacket + + +def dsmSend(devName, baudRate): + global dsmBeat + + logger.info("dsmSend running") + + if opts.sim: + logger.info("not sending to pixhawk") + else: + logger.info("opening %s at %d", devName, baudRate) + try: + serialPort = serial.Serial(devName, baudRate) + except serial.SerialException as excOpen: + logger.error(excOpen.__str__()) + return + + pixOutLogger = None + #pixOutLogger = logd.PixOutLogger() + + while True: + dsmBeat += 1 + + rcDsmLock.acquire() + dsmBytes = dsmPack(rcChansOut) + rcDsmLock.release() + + if dsmBytes is None: + logger.debug("dsmSend: None") + else: + logger.debug("dsmSend: %s", + [hex(ord(c)) for c in dsmBytes].__str__()) + + if dsmBytes is not None and not opts.sim: + if pixOutLogger: + pixOutLogger.log_packet(dsmBytes) + serialPort.write(dsmBytes) + + time.sleep(DSM_INTERVAL) + + +def rcReceive(udpPortNum): + global rcBeat + global rcChansOut + + logger.info("rcReceive running") + + stats = simple_stats.SimpleStats() + + pixInLogger = None + #pixInLogger = logd.PixInLogger() + + # Open socket + logger.info("rcReceive: listening on port %d", udpPortNum) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind(("", udpPortNum)) + + # We require the timestamp on the RC packet to be greater than the + # previous one, or the packet is dropped. However, if we see enough "old" + # packets in a row, we assume the timestamp has restarted, and start + # accepting packets again. + rcTimePrev = None + oldPktCnt = 0 + oldPktRestart = 5 # 100 msec at 20 msec / packet + + nextWarnTime = None + warnInterval = datetime.timedelta(seconds = 5) + + # The sequence number is used to detect and count dropped packets. We + # increment dropCnt for "small" gaps in the sequence (incrementing it by + # the number of packets dropped), and increment discCnt for "large" gaps + # (discontinuities). goodCnt is incremented for packets receive in sequence. + rcSeqPrev = None + dropCnt = 0 + goodCnt = 0 + discCnt = 0 + discGap = 5 + + # Packets received less than this much time after the previous one are + # discarded, under the assumption that it is part of a burst. The last + # packet in a burst is discarded (even if it is not old), and then the + # next one is the first used. + pktIntervalMin = datetime.timedelta(milliseconds=2) + + # Log packet statistics periodically + logInterval = datetime.timedelta(seconds = 10) + logTime = datetime.datetime.now() + logInterval + + rcBeatTime = None + rcBeatIntervalMax = datetime.timedelta(seconds = 0.040) + nowDsm = None + sock.settimeout(SOCKET_TIMEOUT) + while True: + rcBeat += 1 + # The following rcBeatInterval stuff was added because we were getting + # "errors" when checking for rcBeat to increment at least every 1.5 + # times the socket timeout. The error check was loosened up and the + # interval measurement added to see just how bad it is. + if rcBeatTime is not None: + rcBeatInterval = datetime.datetime.now() - rcBeatTime + if rcBeatIntervalMax < rcBeatInterval: + rcBeatIntervalMax = rcBeatInterval + logger.info("rcBeatIntervalMax is now %f seconds", + rcBeatIntervalMax.total_seconds()) + rcBeatTime = datetime.datetime.now() + try: + rcBytes, addr = sock.recvfrom(256) + except socket.timeout: + now = datetime.datetime.now() + if nextWarnTime is None or now >= nextWarnTime: + logger.warn("socket timeout: sending failsafe packet") + nextWarnTime = now + warnInterval + rcTime = 0 + rcSeq = 0 + rcChans = rcFailsafeChans + rcTimePrev = rcTime + else: + nowDsmLast = nowDsm + nowDsm = datetime.datetime.now() + now = nowDsm + if pixInLogger: + pixInLogger.log_packet(rcBytes) + if nowDsmLast is not None: + delta_us = (nowDsm - nowDsmLast).total_seconds() * 1000000 + stats.update(delta_us) + if nextWarnTime is not None: + logger.info("received packet after timeout") + nextWarnTime = None + # only accept RC packets from Artoo + if not addr[0] in rcSourceIps: + logger.warn("packet from %s ignored", addr[0]) + continue + rcTime, rcSeq, rcChans = rcUnpack(rcBytes) + # Check sequence - just require that the timestamp is increasing. + # But... if we see enough "old" packets in a row (oldPktRestart), + # we assume the timestamp has started over and start accepting + # packets again. + if rcTimePrev is not None and \ + rcTime <= rcTimePrev and \ + oldPktCnt < oldPktRestart: + logger.warn("old packet ignored (%s <= %s)", + rcTime.__str__(), rcTimePrev.__str__()) + oldPktCnt += 1 + continue + rcTimePrev = rcTime + oldPktCnt = 0 + + # The packet is later than the previous one; look for missed + # packets (diagnostic). + # 64K packets wraps after about 21m 50s; test with wrap at 256 (5s) + rcSeqMax = 65536 + #rcSeqMax = 256 + #rcSeq = rcSeq & (rcSeqMax - 1) + if rcSeqPrev is None: + rcSeqPrev = rcSeq + else: + # mod-64K subtract + gap = rcSeq - rcSeqPrev + if gap < 0: + gap += rcSeqMax + if gap == 1: + goodCnt += 1 + else: + if gap <= discGap: + dropCnt += (gap - 1) + else: + discCnt += 1 + logger.info("gap=%d good=%d drop=%d disc=%d", + gap, goodCnt, dropCnt, discCnt) + rcSeqPrev = rcSeq + + logger.debug("%s %s %s", + rcTime.__str__(), rcSeq.__str__(), rcChans.__str__()) + + if now > logTime: + logger.info("good=%d drop=%d disc=%d", goodCnt, dropCnt, discCnt) + count = stats.count() + if count > 0: + logger.info("n=%d avg=%0.0f min=%0.0f max=%0.0f stdev=%0.1f", + count, stats.average(), + stats.min(), stats.max(), stats.stdev()) + stats.reset() + logTime += logInterval + + # Make new RC data available to dsmSend thread. rcChans was either set + # to the new RC data if we got it, or to the failsafe packet if not. + rcDsmLock.acquire() + rcChansOut = rcChans + rcDsmLock.release() + + + +parser = optparse.OptionParser("pixrc.py [options]") +parser.add_option("--sim", action="store_true", default=False, + help="do not send to Pixhawk") +(opts, args) = parser.parse_args() + +os.nice(-20) + +# Don't let the RC receive thread update the RC data while the DSM send thread +# is using it +rcDsmLock = threading.Lock() + +if DSM_SEND_ENABLE: + sender = threading.Thread(name = "dsmSend", target = dsmSend, args = (rcDsmDev, rcDsmBaud)) + sender.daemon = True + sender.start() + +receiver = threading.Thread(name = "rcReceive", target = rcReceive, args = (rcDestPort, )) +receiver.daemon = True +receiver.start() + +# When this module exits, the threads will be killed. +# This loop watches to see that both threads are still running, and if either +# stops, it exits, causing init to restart this module. +pollSleep = max(DSM_INTERVAL, SOCKET_TIMEOUT) * 5 +# * 1.5 resulted in occasional errors; added rcBeatInterval logging + +while True: + oldDsmBeat = dsmBeat + oldRcBeat = rcBeat + time.sleep(pollSleep) + if DSM_SEND_ENABLE: + if dsmBeat == oldDsmBeat: + logger.error("dsmSend thread appears to be dead; exiting") + logger.info("dsmBeat=%d pollSleep=%d", dsmBeat, pollSleep) + sys.exit(1) + if rcBeat == oldRcBeat: + logger.error("rcReceive thread appears to be dead; exiting") + logger.info("rcBeat=%d pollSleep=%d", rcBeat, pollSleep) + sys.exit(1) diff --git a/flightcode/old/rssi_send.py b/flightcode/old/rssi_send.py new file mode 100755 index 0000000..720999e --- /dev/null +++ b/flightcode/old/rssi_send.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python + +import os +import socket +import sys +import time +import logging +import logging.config +import iw +from pymavlink import mavutil +sys.path.append("/usr/bin") +import clock + +if_name = "wlan0" +src_system = 10 +src_component = 0 +conf_filename = "/etc/sololink.conf" + + +# 'mav' is a mavutil.mavlink.MAVLink, needed for +# srcSystem, srcComponent, and sequence +# 'dbm' is the signal dbm as read from the wifi card +# return is a mavlink message as a string (binary) +def create_rssi_msg(mav, dbm): + # dbm is a small negative number. We would like the rssi byte in the + # mavlink message to be interpretable as a signed byte, but the mavlink + # stuff packs it as an unsigned byte. Get the right bits into the + # message by munging before packing: + # dbm byte_to_pack + # -128 -> 0x80 -> 128 + # -1 -> 0xff -> 255 + # 0 -> 0x00 -> 0 + # 1 -> 0x01 -> 1 + # +127 -> 0x7f -> 127 + # ...so negative values of dbm are replaced with (256 + dbm) + # This is all just to get the pymavlink struct packer to generate the + # message we want. + + if dbm is None or dbm < -128: + dbm = -128 + if dbm > 127: + dbm = 127 + + if dbm >= 0: + rssi = dbm + else: + rssi = 256 + dbm + + # on Solo, we are "remrssi" + msg = mavutil.mavlink.MAVLink_radio_status_message( + rssi=0, remrssi=rssi, txbuf=0, noise=0, remnoise=0, rxerrors=0, fixed=0 + ) + return msg.pack(mav) + + +# take a mavlink message and print the remrssi +def show_remrssi(m): + rssi = ord(m[11]) + if rssi >= 128: + rssi -= 256 + print rssi + + +logging.config.fileConfig(conf_filename) +logger = logging.getLogger("wifi") + +logger.info("rssi_send starting") + +mav = mavutil.mavlink.MAVLink(None, src_system, src_component) + +# socket to inject into downlink telemetry from +sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) +sock.bind("/tmp/rssi_send." + str(os.getpid())) + +skip_list = [ "mac", "ssid", "freq" ] + +abbrev_list = { + "rxBytes" : "rxb", "rxPackets" : "rxp", + "txBytes" : "txb", "txPackets" : "txp", + "txBitrate" : "txr", "signal" : "sig" +} + +#RC lockout +rclockout = True +#if there is an unlock file or no lock file, we're unlocked +if os.path.isfile("/tmp/.rc_unlock"): + rclockout = False +elif (not os.path.isfile("/etc/.rc_lock")) and (not os.path.isfile("/mnt/rootfs.ro/etc/.rc_lock")): + rclockout = False + +# how often to send a message +interval_us = 1000000 + +next_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + interval_us + +while True: + + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if next_us > now_us: + time.sleep((next_us - now_us) / 1000000.0) + next_us += interval_us + + info = iw.link(if_name) + s = "" + for key in info: + if key in skip_list: + continue + if key in abbrev_list: + lbl = abbrev_list[key] + else: + lbl = key + s = s + str(lbl) + "=" + str(info[key]) + " " + logger.info(s) + dbm = info["signal"] + + #Check the lockout status again + if rclockout is True: + if os.path.isfile("/tmp/.rc_unlock"): + rclockout = False + + if rclockout is False: + try: + m = create_rssi_msg(mav, dbm) + sock.sendto(m, "/run/telem_downlink") + mav.seq += 1 + if mav.seq >= 256: + mav.seq = 0 + except: + pass diff --git a/flightcode/old/runMavproxy.sh b/flightcode/old/runMavproxy.sh new file mode 100755 index 0000000..cebfad2 --- /dev/null +++ b/flightcode/old/runMavproxy.sh @@ -0,0 +1,20 @@ +#!/bin/sh + +# The goal is to use netcat to run MAVProxy as a server so we can connect to +# its console on a tcp port. This does not quite work; one can connect to the +# port from a remote host, but the input and/or output is getting buffered +# somewhere such that output does not show up until the connection is being +# closed, then it all comes at once. + +# In any case, we don't want MAVProxy on the console tty when running from +# init, so this at least serves that purpose. + +# mavproxy not starting up when run by netcat... +#nc -l -p 5020 -e + +mavproxy.py \ + --master=/dev/ttymxc1,57600 --rtscts \ + --out=10.1.1.1:14550 + +# XXX bogs down the telemetry stream +# --load-module=solo \ diff --git a/flightcode/old/simple_stats.py b/flightcode/old/simple_stats.py new file mode 100755 index 0000000..ab7d3f5 --- /dev/null +++ b/flightcode/old/simple_stats.py @@ -0,0 +1,49 @@ +import math + +class SimpleStats: + + def __init__(self): + self.reset() + + def reset(self): + self._count = 0 + self._sum_x = 0.0 + self._sum_x2 = 0.0 + self._max_x = None + self._min_x = None + + def update(self, x): + self._count += 1 + self._sum_x += x + self._sum_x2 += (x * x) + if self._max_x is None or self._max_x < x: + self._max_x = float(x) + if self._min_x is None or self._min_x > x: + self._min_x = float(x) + + # always returns int + def count(self): + return self._count + + # always returns float + def max(self): + return self._max_x + + # always returns float + def min(self): + return self._min_x + + # always returns float + def average(self): + if(self._count > 0): + return float(self._sum_x / self._count) + else: + return 0 + + # always returns float + def stdev(self): + avg = self.average() + if(self._count > 0): + return math.sqrt(self._sum_x2 / self._count - avg * avg) + else: + return 0 diff --git a/flightcode/old/slip.py b/flightcode/old/slip.py new file mode 100755 index 0000000..3b5541b --- /dev/null +++ b/flightcode/old/slip.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python + +import datetime +import logging +import logging.config +import serial + +configFileName = "/etc/sololink.conf" + +logging.config.fileConfig(configFileName) +logger = logging.getLogger("slip") + + + +class slip(): + + + END = chr(0xc0) # indicates end of packet + ESC = chr(0xdb) # indicates byte stuffing + ESC_END = chr(0xdc) # ESC ESC_END means END data byte + ESC_ESC = chr(0xdd) # ESC ESC_ESC means ESC data byte + + + # This is really for detecting garbage input, and not passing huge junk to the + # stm32 message handlers. Intended to be much bigger than any real packet. If + # we get more than this much data without finding an END, we drop the packet + # and go back to looking for sync. + maxPktLen = 1024 + + + def __init__(self, stream): + # I/O interface, provides s=read() and write(s) + self._stream = stream + self._inSync = False + # count of each reason we lost sync + self.desyncCounts = {} + self._desyncLogInterval = datetime.timedelta(seconds=1) # =None to never log + self._desyncLogLast = None + # receive counters + self.counts = { 'BYTE': 0, 'SYNC': 0, 'DROP': 0, 'ESC': 0 } + self._syncLogInterval = datetime.timedelta(seconds=1) # =None to never log + self._syncLogLast = None + + + # Lost sync. Functionally, we just want to set inSync False, but for analysis + # and debugging, we use a function so we can count things and log and such. + def desync(self, reason): + now = datetime.datetime.now() + self._inSync = False + # count various reasons for losing sync (debug) + if reason not in self.desyncCounts: + self.desyncCounts[reason] = 0 + self.desyncCounts[reason] += 1 + # periodically log (debug) + if (self._desyncLogInterval is not None) and \ + ((self._desyncLogLast is None) or \ + ((now - self._desyncLogLast) >= self._desyncLogInterval)): + logger.info("lost sync (desyncs = %s)", str(self.desyncCounts)) + self._desyncLogLast = now + + + # Receive one SLIP-encoded packet. If there is a timeout + # waiting for the next character, any data received so far is dropped and + # (None, None) is returned. The timout is set when the port is opened. + def recv(self): + + pktTime = None + pkt = [] + + if not self._inSync: + now = datetime.datetime.now() + self.counts['SYNC'] += 1 + # periodically log + if (self._syncLogInterval is not None) and \ + ((self._syncLogLast is None) or \ + ((now - self._syncLogLast) >= self._syncLogInterval)): + logger.info("syncing, counts = %s", str(self.counts)) + self._syncLogLast = now + # find the next END + while True: + b = self._stream.read() + if len(b) == 0: + return None, None # timeout + self.counts['BYTE'] += 1 + if b == slip.END: + break + self.counts['DROP'] += 1 + + # already synced, or just got an END + self._inSync = True + + # read packet + while True: + b = self._stream.read() + if len(b) == 0: + return None, None # timeout + self.counts['BYTE'] += 1 + if len(pkt) == 0: + if b == slip.END: + # packet data has not started + continue + else: + # first byte in packet + # timestamp is that of the first byte in the packet + pktTime = datetime.datetime.now() + + # If b is END at this point, it is the packet end. pkt[] must have + # at least one byte, and pktTime must have been set to get past the + # continue a few lines up + if b == slip.END: + return pktTime, pkt + + if b == slip.ESC: + b = self._stream.read() + if len(b) == 0: + return None, None # timeout + self.counts['BYTE'] += 1 + if b == slip.ESC_END: + pkt.append(slip.END) + elif b == slip.ESC_ESC: + pkt.append(slip.ESC) + else: + pkt.append(b) + self.counts['ESC'] += 1 + else: + pkt.append(b) + + # Sanity check + if len(pkt) > slip.maxPktLen: + # Something is wrong + slip.desync('TOO_LONG') + + + # Send one SLIP-encoded packet. + # 'pkt' is anything we can iterate over a byte at a time + # (e.g. string, list of single-char strings) + def send(self, pkt): + + self._stream.write(slip.END) # may be optional + + for c in pkt: + if c == slip.END: + self._stream.write(slip.ESC) + self._stream.write(slip.ESC_END) + elif c == slip.ESC: + self._stream.write(slip.ESC) + self._stream.write(slip.ESC_ESC) + else: + self._stream.write(c) + + self._stream.write(slip.END) + + + +# END class slip + + + +if __name__ == "__main__": + # test + + class dataSource(): + def __init__(self, data): + self._data = data + self._index = 0 + def read(self): + b = self._data[self._index] + self._index += 1 + return b + + # These tests just check the END processing. + # ESC processing has been running for a while; add tests if needed. + + testData = "" + # junk discarded before first END, and the first END + testData += "abc" + slip.END + # first packet + testData += "1:123" + slip.END + # second packet, only the one END between first and second + testData += "2:45" + slip.END + # leading ENDs are okay + testData += slip.END + "3:6" + slip.END + # lots of ENDs are okay + testData += slip.END + slip.END + slip.END + "4:789" + slip.END + + testSer = dataSource(testData) + + while True: + try: + print recv(testSer) + except: + break diff --git a/flightcode/old/stm32.py b/flightcode/old/stm32.py new file mode 100755 index 0000000..747d640 --- /dev/null +++ b/flightcode/old/stm32.py @@ -0,0 +1,812 @@ +#!/usr/bin/env python + +# Receive and handle messages from the STM32: stm32.receiver waits for messages +# from the STM32. When one arrives, strip off the one-byte packet ID, and call +# a handler based on the packet ID. +# +# |--> DSM -----------> stm32_rc +# | +# |--> SYSINFO -------> stm32_sysinfo +# STM32 --> slip --> stm32.receiver -->| +# |--> MAVLINK -------> stm32_mavlink +# | +# |--> PAIR_CONFIRM --> stm32_pairCnf +# +# To send messages to the STM32, write them to a UDP port, where the port +# used determines the message ID used. The stm32.sender function is called +# after each incoming RC packet, and if there is anything in any of the UDP +# ports, it prepends the one-byte message ID and sends it to the STM32. +# +# mavlink --> udp -->| +# | +# sysinfo --> udp -->| +# |--> select --> stm32.sender --> slip --> STM32 +# pairReq --> udp -->| +# | +# pairCnf --> udp -->| +# +# The slip object is given an interface that can send/receive data. Normally +# it is the serial port to the STM32, but for testing it can be a TCP socket +# to the STM32 "simulator" (see stm32_sim.py). + +import ConfigParser +import datetime +import logd +import logging +import logging.config +import os +import re +import select +import serial +import simple_stats +import slip +import socket +import struct +import sys +import threading +import time + +from stm32_defs import * + + +if True: + configFileName = "/etc/sololink.conf" + soloAddressFileName = "/var/run/solo.ip" +else: + print "USING DEBUG FILE NAMES" + configFileName = "./sololink.conf" + soloAddressFileName = "./solo.ip" + + +logging.config.fileConfig(configFileName) +logger = logging.getLogger("stm32") + +logger.info("starting (20141114_1132)") + +config = ConfigParser.SafeConfigParser() +config.read(configFileName) + + +### These could be in a solo subclass of ConfigParser + +def configGetString(optName): + try: + value = config.get("solo", optName) + return value + except ConfigParser.NoOptionError: + logger.error("%s not found in %s", optName, configFileName) + raise # pass it on up + +def configGetInt(optName): + try: + value = config.getint("solo", optName) + return value + except ConfigParser.NoOptionError: + logger.error("%s not found in %s", optName, configFileName) + raise # pass it on up + +### + +### This should be in a subclass of ConfigParser +# Parse string into a list of IP address/port tuples, e.g. +# "" -> [] +# "10.1.1.1:5000" -> [("10.1.1.1", 5000)] +# "10.1.1.1:5000;10.1.1.100:5001" -> [("10.1.1.1", 5000), ("10.1.1.100", 5001)] +def parseIps(stringIps): + if len(stringIps) == 0: + listIps = [] + else: + listIps = re.split(";", stringIps) + for i in range(len(listIps)): + # ipPort should be e.g. "10.1.1.1:5001" + tupleIp = re.split(":", listIps[i]) + tupleIp[1] = int(tupleIp[1]) + listIps[i] = tuple(tupleIp) + return listIps +### + +# Solo's IP address +soloIp = None + +# Default port to which RC packets will be sent (at IP soloIp) +rcDestPort = configGetInt("rcDestPort") + +pktIdToTypeName = [ + "NOP", + "DSM", + "CAL", + "SYSINFO", + "MAVLINK", + "SET_RAW", + "RAW_RPT", + "PAIR_REQ", + "PAIR_CNF", + "PAIR_RES" +] + +# i.MX6 -> STM32 message ports. A message arriving on one of these ports will +# be given the STM32 protocol packet ID and slipped to the STM32. +mavDestPort = configGetInt("mavDestPort") +sysDestPort = configGetInt("sysDestPort") +pairReqDestPort = configGetInt("pairReqDestPort") +pairResDestPort = configGetInt("pairResDestPort") + +# STM32 serial port +stm32Dev = configGetString("stm32Dev") +stm32Baud = configGetInt("stm32Baud") + +# Timeout for reading the next packet from the slip interface +portTimeout = 0.2 + + + +setSoloIpErrorLogged = False + +def setSoloIp(): + """attempt to set solo's ip address + + Pairing module puts solo's address in soloAddressFileName when the + connection is first established. It is expected to be on a non- + persistent file system so it does not exist until solo is detected + after each boot. + """ + global soloIp + + try: + f = open(soloAddressFileName) + except IOError: + # can't open file; no connection to solo yet + return False + fileData = f.read() + f.close() + + # fileData should simply be an IP address + soloIp = fileData.strip() + logger.info("using solo IP address %s", soloIp) + + return True + + + +# A UDP socket that has an associated packet ID. The idea is that each type +# of outgoing packet has an associated UDP port. To send a packet of a +# particular type, a message is sent to that type's port. The sender thread +# can then select on a set of UDP sockets, and when data arrives on one, it +# can get the packet ID associated with that socket. +class idSocket(socket.socket): + def __init__(self, id): + socket.socket.__init__(self, socket.AF_INET, socket.SOCK_DGRAM) + self._id = id + def id(self): + return self._id + + + +class stm32(): + + + def __init__(self, port, logPipe): + self._slip = slip.slip(port) + self._stats = simple_stats.SimpleStats() + # The MAVLink uplink address is learned the first time we get a + # MAVLink packet from Solo. + self._mavUplinkAddress = None + # The pairing socket address is learned the first time we get a + # pair request packet from the pairing server module. + self._pairingAddress = None + # Message handlers. + # Receiver thread uses these to process messages received from STM32. + self.rc = stm32_rc(self) + self.mavlink = stm32_mavlink(self) + self.sysinfo = stm32_sysinfo(self) + self.pairCnf = stm32_pairConfirm(self) + # debug counts + self._msgInCounts = {} + self._msgOutCounts = {} + self._junkCounts = {} + # control how often we log progress and junk messages + self._logProgressInterval = datetime.timedelta(seconds=30) # =None to never log + self._logProgressLast = None + self._logJunkInterval = datetime.timedelta(seconds=5) # =None to never log + self._logJunkLast = None + # Packet log + self._logPipe = logPipe + # Sender thread (sends messages to STM32) + self._inSocks = [ ] + # A packet arriving on one of these UDP ports is sent to the STM32 + # with the given packet ID + self.addInput(mavDestPort, PKT_ID_MAVLINK) + self.addInput(sysDestPort, PKT_ID_SYSINFO) + self.addInput(pairReqDestPort, PKT_ID_PAIR_REQUEST) + self.addInput(pairResDestPort, PKT_ID_PAIR_RESULT) + # Receiver thread (receives messages from STM32) + self._quit = False + self._timeout = False + self._receiver = threading.Thread(name = 'stm32.receiver', + target = stm32.receiver, + args = (self, )) + self._receiver.daemon = True + self._sender = threading.Thread(name = 'stm32.sender', + target = stm32.sender, + args = (self, )) + self._sender.daemon = True + # Start thread + self._receiver.start() + self._sender.start() + + + def exit(self): + self._quit = True + self._receiver.join(1.0) + if self._receiver.isAlive(): + logger.error("timeout waiting for receiver thread to exit") + self._sender.join(1.0) + if self._sender.isAlive(): + logger.error("timeout waiting for sender thread to exit") + + + def logProgress(self, msgInType, msgOutType): + # count incoming messages of each type + if msgInType is not None: + if msgInType not in self._msgInCounts: + self._msgInCounts[msgInType] = 0 + self._msgInCounts[msgInType] += 1 + # count outgoing messages of each type + if msgOutType is not None: + if msgOutType not in self._msgOutCounts: + self._msgOutCounts[msgOutType] = 0 + self._msgOutCounts[msgOutType] += 1 + # periodically print counts + now = datetime.datetime.now() + if (self._logProgressInterval is not None) and \ + ((self._logProgressLast is None) or \ + ((now - self._logProgressLast) >= self._logProgressInterval)): + logger.info("messages in: %s", str(self._msgInCounts)) + logger.info("messages out: %s", str(self._msgOutCounts)) + if self._stats.count() > 0: + logger.info("dsm: n=%d avg=%0.0f min=%0.0f max=%0.0f stdev=%0.1f", + self._stats.count(), self._stats.average(), + self._stats.min(), self._stats.max(), + self._stats.stdev()) + self._stats.reset() + self._logProgressLast = now + + + def logJunk(self, junkType): + now = datetime.datetime.now() + if junkType not in self._junkCounts: + self._junkCounts[junkType] = 0 + self._junkCounts[junkType] += 1 + if (self._logJunkInterval is not None) and \ + ((self._logJunkLast is None) or \ + ((now - self._logJunkLast) >= self._logJunkInterval)): + logger.info("junk: stm32 %s, slip %s", + str(self._junkCounts), str(self._slip.counts)) + self._logJunkLast = now + + + def logPacket(self, pktTime, pkt): + # don't log unless Solo is connected + if soloIp is None: + return + logd.logStm32Packet(pktTime, pkt) + + + # Receiver thread waits for a packet to arrive from the STM32 over the + # serial port, then based on the packet ID, gives it to one of the message + # handlers. This thread is normally blocked on the serial port; the + # message handlers should not block. + # + # If the recv function returns (None, None), then there was a timeout + # waiting for data. + def receiver(self): + logger.info("stm32.receiver running") + + self.packetLogger = None + #self.packetLogger = logd.Stm32Logger(self._logPipe) + + if self.packetLogger: + now = datetime.datetime.now() + self.packetLogger.log_packet(now, "\0start " + str(now)) + + pktTimeDsmLast = None + + while True: + + pktTime, pkt = self._slip.recv() + + if self._quit: + break + + if pktTime is None or pkt is None: + self.logJunk("TIMEOUT") + self._timeout = True + continue + self._timeout = False + + if len(pkt) < 1: + self.logJunk("ZERO") + continue + + if self.packetLogger: + self.packetLogger.log_packet(pktTime, "".join(pkt)) + + pktId = ord(pkt[0]) + + try: + typeName = pktIdToTypeName[pktId] + except: + logger.info("stm32.receiver: %s", [hex(ord(c)) for c in pkt]) + typeName = "UNKNOWN" + + if pktId == PKT_ID_NOP: + self.logJunk(typeName) + elif pktId == PKT_ID_DSM: + self.rc.handle(pktTime, pkt[1:]) + # update stats + if pktTimeDsmLast is not None: + delta_us = (pktTime - pktTimeDsmLast).total_seconds() * 1000000 + self._stats.update(delta_us) + pktTimeDsmLast = pktTime + # handle outgoing messages +# self.ender() + elif pktId == PKT_ID_CAL: + pass + elif pktId == PKT_ID_SYSINFO: + self.sysinfo.handle(pktTime, pkt[1:]) + elif pktId == PKT_ID_MAVLINK: + # Upstream MAVLink (Artoo -> Solo) + self.mavlink.handle(pktTime, pkt[1:]) + elif pktId == PKT_ID_PAIR_CONFIRM: + self.pairCnf.handle(pktTime, pkt[1:]) + elif pktId == PKT_ID_SHUTDOWN_REQUEST: + #Shutdown gracefully... + logger.info("received shutdown request"); + os.system("shutdown -h now") + else: + # Don't desync here. We did just get an END, after all. + self.logJunk(typeName) + + self.logProgress(typeName, None) + + # end while True + + if self.packetLogger: + now = datetime.datetime.now() + self.packetLogger.log_packet(now, "\0exit " + str(now)) + self.packetLogger.uninit() + + logger.info("stm32.receiver exiting") + + + # Sender thread waits for packets to arrive on any of the UDP ports. When + # a message arrives, it is prepended with the packet ID and sent to the + # STM32. This is the only thread that is allowed to write to the serial + # port. This thread is normally blocked in the select on the UDP ports. + # If there is regular traffic on at least one of the UDP ports, then the + # _quit request will be noticed and the thread will exit when requested. + # Otherwise, it will be killed when the main module terminates, and could + # cause an ugly but harmless exception on the way out. The select could + # be done with a time to mitigate this, but that's TBD (the normal case is + # that MAVLink is coming in often enough so the timeout is not needed). + def sender(self): + + while True: + + if self._quit: + break + + ready = select.select(self._inSocks, [], []) #Blocking + + for sock in ready[0]: + + pkt, srcAdrs = sock.recvfrom(4096) + + pktId = sock.id() + + try: + typeName = pktIdToTypeName[pktId] + except: + typeName = "UNKNOWN" + + if self._mavUplinkAddress is None and pktId == PKT_ID_MAVLINK: + # Save source address; it is the destination address for + # the mavlink uplink. + # srcAdrs is a ("string", port) tuple. + self._mavUplinkAddress = srcAdrs + logger.info("mavlink destination address: %s", + str(self._mavUplinkAddress)); + + if self._pairingAddress is None and pktId == PKT_ID_PAIR_REQUEST: + # Save source address; pairing confirm will go there + self._pairingAddress = srcAdrs + logger.info("pairing destination address: %s", + str(self._pairingAddress)); + + self._slip.send(chr(pktId) + pkt) + + self.logProgress(None, typeName) + + ### end for sock in ready[0] + ### End while True + + + # Add a new UDP port to listen on. When a packet arrives, it will be + # forwarded to the STM32 with the supplied packet ID. All addInput() calls + # are expected to be done before the first call to sender(). + def addInput(self, portNum, pktId): + sock = idSocket(pktId) + sock.bind(('', portNum)) + self._inSocks.append(sock) + + + +class stm32_sysinfo(): + + def __init__(self, owner): + self._owner = owner + self._uniqueId = None + self._hwVersion = None + self._swVersion = None + self.updateTime = datetime.datetime.now() + + + def handle(self, pktTime, pkt): + """Handle one "sysinfo" message from STM32. + + The packet ID has already been stripped from the front. The packet as + received here looks like this (names are from artoo source): + + --DATA-------------- --LENGTH-------------------- --AS OF 9/25/14--- + Sys::UniqueId Sys::UniqueIdLen 12 bytes + Sys::HardwareVersion sizeof(Sys::HardwareVersion) 2 bytes + Version::str() strlen(Version::str()) variable + -------------------- ---------------------------- ------------------ + + The UniqueId and HardwareVersion are (9/25/14) compiled-in, and the + version string is generated at build time from `git describe --tags`. + """ + pktLen = len(pkt) + now = datetime.datetime.now() + if pktLen > 0: + + if pktLen < 14: + # malformed packet received - log it + self._owner.logJunk("BAD_SYS") + return + + newSysinfo = "".join(pkt) + newUniqueId = newSysinfo[:12] + # struct.unpack_from always returns a tuple + newHwVersion = struct.unpack_from('= self._dbg_printInterval_us): + interval_us = pktTime_us - self._timePrev_us + msg = "" + if self._dbg_printDetails & stm32_rc.PRINT_INTERVALS: + msg += str(pktTime_us - self._timePrev_us) + if self._dbg_printDetails & stm32_rc.PRINT_PACKETS: + msg += str(struct.unpack(" 21000: + msg += "GAP = " + str(interval_us) + " us" + if msg != "": + logger.info(msg) + #print msg + self._dbg_printLast_us = pktTime_us + + self._sock.sendto(s, self._ipAddress) + + self._timePrev_us = pktTime_us + self._sequence = (self._sequence + 1) % 0xffff + + + +class stm32_mavlink(): + + def __init__(self, owner): + self._owner = owner + self._destAddress = None + self._destSock = None + + def handle(self, pktTime, pkt): + """Handle one MAVLink packet from STM32. + + Forwarded to Solo (Pixhawk) if we know the UDP port number to send to. + The UDP port number is captured and saved the first time we get a + MAVLink packet from Solo. Until then, uplink MAVLink packets are + dropped, with an error logged. + """ + if self._destAddress is None: + self._destAddress = self._owner._mavUplinkAddress + + # _destAddress is still None if _mavUplinkAddress is still None + # (no MAVLink received from Solo yet) + if self._destAddress is None: + logger.error("MAVLink packet from STM32 dropped (no destination)") + logger.error("%s", [hex(ord(c)) for c in pkt].__str__()) + else: + # create socket on first use + if self._destSock is None: + self._destSock = socket.socket(socket.AF_INET, + socket.SOCK_DGRAM) + self._destSock.sendto("".join(pkt), self._destAddress) + + +class stm32_pairConfirm(): + + def __init__(self, owner): + self._owner = owner + + def handle(self, pktTime, pkt): + """Handle pair confirm packet from the STM32.""" + logger.info("pair confirm for \"%s\"", "".join(pkt)) + if self._owner._pairingAddress is None: + logger.error("pairing address not set! (dropping pair confirm)") + else: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + confirm = struct.pack("!BBB", 3, 1, 1) + sock.sendto(confirm, self._owner._pairingAddress) + + + +# Provide read() and write() methods identical to those of a serial port. +# Mainly, read() defaults to one byte. This is for testing when we are using +# the simulated STM32 via TCP. +class sock1(): + def __init__(self, sock): + self._sock = sock + def read(self): + # socket raises exception when it times out; serial port just + # returns no data + try: + b = self._sock.recv(1) + except socket.timeout: + b = "" + return b + def write(self, c): + self._sock.sendall(c) + + + +def createTcpPort(address): + logger.info("connecting to %s", str(address)) + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.connect(address) + sock.settimeout(portTimeout) + return sock1(sock) + + + +def createSerialPort(device, baud): + try: + logger.info("opening %s at %d", device, baud) + # Opening the port may raise serial.SerialException; let that pass on + # up. The timeout is intended to be several times the minimum packet + # interval, i.e. for RC packets arriving every 20 msec, a timeout of + # 200 msec (x10) should work to detect when data stops coming in. + return serial.Serial(port=device, baudrate=baud, timeout=portTimeout) + except serial.SerialException as excOpen: + logger.error(excOpen.__str__()) + sys.exit(1) + except: + logger.error(sys.exc_info()[0]) + sys.exit(1) + + + +def main(port, logPipe): + + # 'port' is anything with .read() and .write() methods, where .read returns + # one byte, and .write writes however many bytes are supplied. + s = stm32(port, logPipe) + + # Ping the STM32 periodically to make sure everything is okay, including + # the send thread, the receive thread, and the STM32 itself. If any pieces + # are stuck, this script exits, killing both threads, and init restarts + # it all. + + # We use the sysinfo message as a "ping". Something like a real ping (with + # less data returned) might be better. + + pingSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + pingInterval = datetime.timedelta(seconds=1) + + # can't do timedelta * float + pingTimeout = 3 * pingInterval + pingInterval / 2 + + while True: + pingSock.sendto('', ('127.0.0.1', sysDestPort)) + time.sleep(pingInterval.total_seconds()) + age = datetime.datetime.now() - s.sysinfo.updateTime + # Can miss 2, but missing 3 means something is wrong + # If none have been missed, "age" will be approximately 1; could be just + # less than 1 or just more than 1. Similar for one missed; age will be + # about 2, or two missed; age will be about 3. That is why 3.5 + # (calculated above) is the threshold to detect three missed. + if age > pingTimeout: + # Exiting this thread causes the stm32 threads to be terminated; + # the init process should restart everything + break + + # Tell the receiver thread to quit gracefully, then wait for it do do so + s.exit() + + logger.error("stm32 not responding; exiting") + + + +if __name__ == "__main__": + from optparse import OptionParser + + parser = OptionParser("stm32.py [options]") + parser.add_option("--tcp", dest="tcpAdrs", type="string", default=None, + help="STM32 at TCP address:port") + parser.add_option("--rc", dest="rcAdrs", type="string", default=None, + help="destination address:port for RC packets") + parser.add_option("--log", dest="logPipe", type="string", default=None, + help="packet log pipe file name") + (opts, args) = parser.parse_args() + + if opts.tcpAdrs: + # opts.tcpAdrs should be of the form "127.0.0.1:55055" + address = parseIps(opts.tcpAdrs) + # address is a list of tuples, each ("ipAdrs", portNum) - use 1st one + port = createTcpPort(address[0]) + else: + port = createSerialPort(stm32Dev, stm32Baud) + + if opts.rcAdrs: + # opts.rcAdrs should be of the form "10.0.0.1:5005" + address = parseIps(opts.rcAdrs) + # address is a list of tuples, each ("ipAdrs", portNum) - use 1st one + soloIp = address[0][0] + + if opts.logPipe: + logPipe = opts.logPipe + else: + logPipe = "/var/run/logd" + + main(port, logPipe) + +# Testing at the python prompt: +# +# >>> import stm32 +# >>> port = stm32.createTcpPort(('127.0.0.1', 55055)) +# >>> stm32.main(port) diff --git a/flightcode/old/stm32_sim.py b/flightcode/old/stm32_sim.py new file mode 100755 index 0000000..d5be0ba --- /dev/null +++ b/flightcode/old/stm32_sim.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python + +import datetime +import slip +import socket +import struct +import threading +import time +from optparse import OptionParser +from pymavlink import mavutil + + +DEFAULT_PORT = 55055 + +RC_PKT_INTERVAL = datetime.timedelta(seconds=0, milliseconds=20) + +rcChannels = [1500, 1500, 1500, 1500, 0, 1500, 0, 0] + +# Packet IDs. These must match "enum MsgID" in artoo/src/hostprotocol.h. +PKT_ID_NOP = 0 +PKT_ID_DSM = 1 +PKT_ID_CAL = 2 +PKT_ID_SYSINFO = 3 +PKT_ID_MAVLINK = 4 +PKT_ID_SET_RAW_IO = 5 +PKT_ID_RAW_IO_REPORT = 6 +PKT_ID_PAIR_REQUEST = 7 +PKT_ID_PAIR_CONFIRM = 8 +PKT_ID_PAIR_RESULT = 9 + + +# Protection for multiple threads calling slip.send(). +# slip.send() does not need to be thread safe in the "real" version, so we +# don't burden it with the lock. +slipSendLock = threading.Lock() + + +# RC packet thread +# Read global control settings and emit packets at RC packet rate +def rcThreadRun(slipIf): + nextPacketTime = datetime.datetime.now() + RC_PKT_INTERVAL + while True: + now = datetime.datetime.now() + while nextPacketTime > now: + sleepTime = (nextPacketTime - now).total_seconds() + time.sleep(sleepTime) + now = datetime.datetime.now() + nextPacketTime += RC_PKT_INTERVAL + packet = chr(PKT_ID_DSM) + for channel in range(8): + data = rcChannels[channel] + packet += chr(data % 256) + packet += chr(data / 256) + try: + slipSendLock.acquire() + slipIf.send(packet) + slipSendLock.release() + except: + break + try: + slipSendLock.release() + except: + pass + + +# Message thread +# Wait for and respond to incoming messages +def msgThreadRun(slipIf): + while True: + pktOut = None + try: + pktTime, pkt = slipIf.recv() + except: + break + if not pkt: + break + pktId = ord(pkt[0]) + if pktId == PKT_ID_NOP: + pass # print "PKT_ID_NOP" + elif pktId == PKT_ID_DSM: + pass # print "PKT_ID_DSM" + elif pktId == PKT_ID_CAL: + pass # print "PKT_ID_CAL" + elif pktId == PKT_ID_SYSINFO: + pass # print "PKT_ID_SYSINFO" + pktOut = chr(PKT_ID_SYSINFO) + "012345678901" + \ + struct.pack(' +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "RcCommander.h" + +using namespace std; + +// Handle ATTACH or DETACH command. +// +// This overrides Commander::handle_attach() to allow for attaching/detaching +// the RC uplink. If the optional argument does not specify the uplink (or is +// not present), Commander::handle_attach() is called to take care of things. +void RcCommander::handle_attach(const struct sockaddr *src_addr, socklen_t src_addr_len, + const char *client, bool attach) +{ + + // If the optional argument is not present or does not specify the uplink + // Commander::handle_attach() is called to take care of things. + if (client == NULL || strcasecmp(client, "UPLINK") != 0) { + Commander::handle_attach(src_addr, src_addr_len, client, attach); + } else { + // Attach/detach the uplink + uplink_attached = attach; + + if (uplink_attached) + syslog(LOG_INFO, "cmd: uplink attached"); + else + syslog(LOG_INFO, "cmd: uplink detached"); + + send_response(src_addr, src_addr_len, "OK\n"); + } + +} // RcCommander::handle_attach diff --git a/flightcode/pixrc/RcCommander.h b/flightcode/pixrc/RcCommander.h new file mode 100644 index 0000000..af53983 --- /dev/null +++ b/flightcode/pixrc/RcCommander.h @@ -0,0 +1,23 @@ + +#include +#include "Commander.h" + +class RcCommander : public Commander +{ +public: + RcCommander(const char *sock_name) : Commander(sock_name), uplink_attached(true){}; + + inline bool is_attached(void) + { + return uplink_attached; + } + +protected: + // override attach/detach to handle the special case of attaching + // or detaching the RC uplink + virtual void handle_attach(const struct sockaddr *, socklen_t, const char *, bool); + +private: + bool uplink_attached; + +}; // class RcCommander diff --git a/flightcode/pixrc/pixrc.cpp b/flightcode/pixrc/pixrc.cpp new file mode 100644 index 0000000..8d2f035 --- /dev/null +++ b/flightcode/pixrc/pixrc.cpp @@ -0,0 +1,1158 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "INIReader.h" +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "RcLock.h" +#include "mutex.h" +#include "net_stats.h" +#include "RcCommander.h" +#include "rc_ipc.h" +#include "file_util.h" + +using namespace std; + +/*********************************************************************** +Timeouts + +NOTE on times: +Timeout detection is all done using CLOCK_MONOTONIC, while packet +timestamps originate (on the controller) from CLOCK_REALTIME. When the +system time is set (normally via GPS some time after startup), the +packet timestamps will generally jump in time, but timeout detection +should not be affected. +***********************************************************************/ +#define LOG_DT_US 10000000 // 10s +#define RC_TIMEOUT_US 400000 // 400ms +#define SM_TIMEOUT_US 400000 // 400ms + +// Used as the socket timeout on the RC uplink socket. This determines the +// resolution with which we will detect timeouts; it does not affect packet +// latency. It could be probably be longer. +// NOTE: The socket timeout appears to work such that the return from +// recvfrom happens on the next kernel tick after the requested timeout. +#define UDP_PERIOD_US 50000 // 50 msec + +// Thread is stuck if it does not run for this long. This won't trigger if +// we have a long gap between packets since the recvfrom timeout should let +// the thread run. +#define UDP_STUCK_US 1000000 // 1 sec + +// Time the udp thread was last activated. The thread should run every +// UDP_PERIOD_US (allowing for scheduling slop). +// This is shared with the main thread. Writes from this thread and reads +// from the main thread must be atomic. +static volatile uint64_t udp_run_us = 0; + +static volatile uint64_t udp_run_interval_max_us = 0; + +// Sleep delay in serial thread. This is approximately how often the +// serial thread will wake up and send whatever is in the packet +// shared memory to the Pixhawk. +#define SER_PERIOD_US 20000 // 20 msec + +// Thread is stuck if it does not run for this long +#define SER_STUCK_US 1000000 // 1 sec + +// Time the serial thread was last activated. The thread should run every +// SER_PERIOD_US (allowing for scheduling slop). +// This is shared with the main thread. Writes from this thread and reads +// from the main thread must be atomic. +static volatile uint64_t ser_run_us = 0; + +// This should be mutex-protected, but we are trying to avoid blocking. +// The worst that can happen, which is astronomically unlikely, is a +// bogus interval might be printed. +static volatile uint64_t ser_run_interval_max_us = 0; + +/*********************************************************************** +RC timeout max. This is pulled from sololink.conf or defaults to +RC_TIMEOUT_US +***********************************************************************/ +static uint64_t rc_timeout_max_us; + +// Shotmanager timeout - if the shared mem packet is not updated in +// this long, trigger failsafe. +static uint64_t sm_timeout_max_us; + +/*********************************************************************** +Serial macros +***********************************************************************/ +static string serialPortName; + +/*********************************************************************** +Thread IDs +***********************************************************************/ +static pthread_t serial_ctx; +static pthread_t UDP_ctx; + +/*********************************************************************** +Thread priorities - these are used with SCHED_FIFO. A higher number is a +higher priority. When priorities are viewed on the target (in /proc, or +using lsproc.py), the number seen there will be (1 - number_here), so +e.g. if PRIORITY is 50, it will show up as priority -51. +***********************************************************************/ +#define UDP_PRIORITY 59 +#define SERIAL_PRIORITY 58 + +/*********************************************************************** +File descriptors +***********************************************************************/ +static int sock_fd; +static int serial_fd; + +/*********************************************************************** +A vector of IPs from whom the artoo will accept RC data +***********************************************************************/ +static vector< string > sourceIPs; + +/*********************************************************************** +The port which we accept RC data from +***********************************************************************/ +static int sourcePort; + +/*********************************************************************** + Commander processor +***********************************************************************/ +static RcCommander *command = NULL; + +/*********************************************************************** + rc lockout +***********************************************************************/ +static bool rc_locked = false; + +/*********************************************************************** +Function: void lock_thread_mem(uint32_t memsize) + +Description: Locks memory in the thread so that stack faults don't cause + timing jitter. +***********************************************************************/ +static void lock_thread_mem(uint32_t memsize) +{ + uint8_t stackmem[memsize]; + + mlockall(MCL_CURRENT | MCL_FUTURE); + + memset(stackmem, 0, sizeof(stackmem)); +} + +/*********************************************************************** +Task debug options +***********************************************************************/ + +//#define INCLUDE_BUG_SIM + +#ifdef INCLUDE_BUG_SIM + +#pragma message("BUILDING WITH INCLUDE_BUG_SIM") + +// Detect bug simulation trigger. +// To simulate a bug, log in and (e.g.) "touch /tmp/pixrc_udp_bug" +static bool test_bug_trigger(const char *trigger_file) +{ + if (file_exists(trigger_file)) { + unlink(trigger_file); + return true; + } else { + return false; + } +} + +// Simulate crash on trigger +static void test_bug_exit(const char *trigger_file) +{ + if (test_bug_trigger(trigger_file)) { + syslog(LOG_INFO, "udp: injecting crash"); + pthread_exit(NULL); + } +} + +// Simulate hang on trigger +static void test_bug_hang(const char *trigger_file) +{ + if (test_bug_trigger(trigger_file)) { + syslog(LOG_INFO, "udp: injecting hang"); + while (1) + sleep(1); + } +} + +#else // INCLUDE_BUG_SIM + +#define test_bug_exit(A) +#define test_bug_hang(A) + +#endif // INCLUDE_BUG_SIM + +/*********************************************************************** +Function: int UDP_setup(void) + +Description: Sets up the UDP receive port. Returns 0 in the event of + an error, 1 otherwise. +***********************************************************************/ +static int UDP_setup(void) +{ + struct sockaddr_in myaddr; /* our address */ + struct timeval timeout; + + /* create a UDP socket */ + if ((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + syslog(LOG_ERR, "udp: can't create socket"); + return 0; + } + + /* Socket timeout */ + timeout.tv_sec = UDP_PERIOD_US / 1000000; + timeout.tv_usec = UDP_PERIOD_US % 1000000; + if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout)) != 0) { + syslog(LOG_ERR, "udp: setsockopt: %s", strerror(errno)); + return 0; + } + + /* Bind the socket to any IP, but we'll check the source later */ + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(sourcePort); + + if (bind(sock_fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + syslog(LOG_ERR, "udp: bind: %s", strerror(errno)); + return 0; + } + + syslog(LOG_INFO, "udp: listening on port %d", sourcePort); + return 1; +} + +/*********************************************************************** +Function: void *UDP_task(void*) + +Description: The main UDP thread task. This runs every 20ms and checks + for the right amount of RC channel data on the UDP port. + It issues a timeout (setting throttle low and all other + stick values to mid) if no new data is received. Incoming + data is also timestamped and recorded for logging. +***********************************************************************/ +static void *UDP_task(void *) +{ + void *rc_ipc_id; + unsigned put_err = 0; + uint64_t put_err_time_us = 0; + unsigned recv_err = 0; + uint64_t recv_err_time_us = 0; + int recvlen; /* # bytes received */ + struct rc_pkt pkt; + uint16_t lastSequence = 0; + uint64_t last_us, now_us; + uint64_t last_run_us = 0; + bool timeout = false; /* indicates an RC failsafe */ + struct sockaddr_in srcAddr; + socklen_t slen; + vector< string >::iterator it; + bool sourceOK; + int interval_us; + uint64_t last_pkt_timestamp = 0; + struct stats { + long int max; + long int avg; + long int avg_n; + long int unknown; // Packet from an unknown sender + long int outOfOrder; + long int dropped; + } packetStats; + unsigned udp_rx_queue_max = 0; // max udp receive queue + uint64_t log_time_us = 0; + + test_bug_exit("/tmp/pixrc_udp_bug"); // simulate crash on startup + + lock_thread_mem(32768); + + // Attach to shared packet memory + rc_ipc_id = rc_ipc_attach(1); + if (rc_ipc_id == NULL) { + syslog(LOG_ERR, "udp: can't attach to rc packet shared memory"); + pthread_exit(NULL); + } + + memset(&pkt, 0, sizeof(pkt)); + + memset(&packetStats, 0, sizeof(packetStats)); + + last_us = clock_gettime_us(CLOCK_MONOTONIC); + + log_time_us = last_us + LOG_DT_US; + + bool is_attached_last = true; + + while (true) { + test_bug_hang("/tmp/pixrc_udp_bug"); // simulate hang + + // If there are packets waiting at this point, then they are being delayed + udp_info_t udp_info; + if (udp_info_get(sourcePort, &udp_info) != 0) { + // log this error once + static bool logged = false; + if (!logged) { + logged = true; + syslog(LOG_ERR, "udp: can't get udp stats"); + } + } else { + if (udp_rx_queue_max < udp_info.rx_queue) + udp_rx_queue_max = udp_info.rx_queue; + } + + // Attempt to receive data + slen = sizeof(srcAddr); + recvlen = recvfrom(sock_fd, &pkt, sizeof(pkt), 0, (sockaddr *)&srcAddr, &slen); + + // Time packet received or timeout detected + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (last_run_us != 0 && udp_run_interval_max_us < (now_us - last_run_us)) + udp_run_interval_max_us = now_us - last_run_us; + last_run_us = now_us; + __atomic_store_n(&udp_run_us, now_us, __ATOMIC_RELAXED); + + if (recvlen == sizeof(pkt)) { + // check the source address to make sure its in our list of source IPs. + sourceOK = false; + for (it = sourceIPs.begin(); it != sourceIPs.end(); ++it) { + // strcmp to avoid creating string object for srcAddr.sin_addr + if (strcmp(it->c_str(), inet_ntoa(srcAddr.sin_addr)) == 0) { + sourceOK = true; + break; + } + } + + // Add it to the stats if its a bad IP + if (!sourceOK) { + packetStats.unknown++; + } else { + // Store some packet statistics + interval_us = now_us - last_us; + packetStats.avg = + (interval_us + (packetStats.avg_n * packetStats.avg)) / (packetStats.avg_n + 1); + ++packetStats.avg_n; + packetStats.max = (interval_us > packetStats.max ? interval_us : packetStats.max); + + // if (interval_us > 100000) + // syslog(LOG_INFO, "udp: packet interval %d usec", interval_us); + + last_us = now_us; + + // Check our failsafe status + if (timeout) { + // Since we got the right amount of data, we assume its OK + syslog(LOG_INFO, "udp: failsafe end after %d ms", interval_us / 1000); + timeout = false; + } + + // See if we dropped one. This doesnt mean we shouldn't + // use the data, because its probably the latest + if (pkt.sequence - lastSequence > 1) // Ignoring the out-of-order + { + packetStats.dropped++; + } + lastSequence = pkt.sequence; + + // Check to see if the data is in the correct order. If it + // appears to be out of order, just log it in the stats, + // but still use it. Out of order should be extremely + // unlikely, and reliably discarding packets that appear + // old - and correctly resuming when they are not - seems + // more complicated (prone to error) than it is worth. + if (pkt.timestamp < last_pkt_timestamp) { + packetStats.outOfOrder++; + } + + // Only write to shared memory if the uplink has not been + // detached by some external client (who should be taking + // the packets received via send_clients, fiddling with + // them, then writing them to shared memory itself). + if (command->is_attached()) { + // Write it to shared memory for serial thread + if (rc_ipc_put(rc_ipc_id, &pkt, 1) != 0) { + put_err++; + if (now_us - put_err_time_us > 10000000) { + syslog(LOG_ERR, + "udp: can't write to rc packet shared memory (count=%d)", + put_err); + put_err_time_us = now_us; + } + } + if (!is_attached_last) { + // just transitioned to attached + syslog(LOG_INFO, "udp: attached"); + is_attached_last = true; + } + } else { + // not attached, don't send it to shared memory + if (is_attached_last) { + // just transitioned to not attached + syslog(LOG_INFO, "udp: detached"); + is_attached_last = false; + } + } + + // Send it to other solo-resident clients + command->send_clients(&pkt, sizeof(pkt)); + + last_pkt_timestamp = pkt.timestamp; + } + } else { + // timeout is the only non-pktlen return we expect + // (recvlen == 1 && errno == EAGAIN) + // if anything else, log it + int err = errno; + if (recvlen != -1 || err != EAGAIN) { + // mystery error + recv_err++; + // Log once per second max. Note if there is more than one + // kind of error happening (multiple combos of recvlen, err), + // not all will necessarily be logged. + if (now_us - recv_err_time_us > 1000000) { + syslog(LOG_ERR, "udp: recv returned %d; errno=%d; recv_err=%d", recvlen, err, + recv_err); + recv_err_time_us = now_us; + recv_err = 0; + } + } + } + + if (!timeout) { + uint64_t age_us; + + // last_us gets updated when we have a good set of data + + age_us = now_us - last_us; + if (age_us > rc_timeout_max_us) { + pkt.channel[0] = 900; // Throttle failsafe value + // Center the stick + pkt.channel[1] = 1500; + pkt.channel[2] = 1500; + pkt.channel[3] = 1500; + // Leave all other channels at their current state + + if (command->is_attached()) { + // Write it to shared memory for serial thread + if (rc_ipc_put(rc_ipc_id, &pkt, 1) != 0) { + put_err++; + if (now_us - put_err_time_us > 10000000) { + syslog(LOG_ERR, + "udp: can't write to rc packet shared memory (error %d)", + put_err); + put_err_time_us = now_us; + } + } + } + + // Send it to other solo-resident clients + command->send_clients(&pkt, sizeof(pkt)); + + timeout = true; + + syslog(LOG_INFO, "udp: failsafe start after %llu ms of inactivity", age_us / 1000); + } + } + + // log time? + if (now_us >= log_time_us) { + syslog(LOG_INFO, "a:%ld,m:%ld,n:%ld,u:%ld,d:%ld,o:%ld,q:%u,udp=%llu,ser=%llu", + (packetStats.avg + 500) / 1000, // a: + (packetStats.max + 500) / 1000, // m: + packetStats.avg_n, // n: + packetStats.unknown, // u: + packetStats.dropped, // d: + packetStats.outOfOrder, // o: + udp_rx_queue_max, // q: + udp_run_interval_max_us, // udp= + ser_run_interval_max_us); // ser= + + memset(&packetStats, 0, sizeof(packetStats)); + udp_rx_queue_max = 0; + udp_run_interval_max_us = 0; + ser_run_interval_max_us = 0; + + log_time_us += LOG_DT_US; + } + + } // while (true) + + pthread_exit(NULL); + +} // UDP_task + +/********************************************************************** +Function: int start_UDP_thread() + +Description: Starts the UDP thread. +***********************************************************************/ +static int start_UDP_thread(void) +{ + int ret = 0; + + // Start the UDP thread + pthread_create(&UDP_ctx, NULL, UDP_task, NULL); + + pthread_setname_np(UDP_ctx, "pixrc_udp"); + +#ifdef UDP_PRIORITY + // UDP priority and schedule + struct sched_param param; + memset(¶m, 0, sizeof(param)); + param.sched_priority = UDP_PRIORITY; + if ((ret = pthread_setschedparam(UDP_ctx, SCHED_FIFO, ¶m))) { + syslog(LOG_ERR, "udp: pthread_setschedparam returned %d", ret); + } +#endif // UDP_PRIORITY + + return ret; +} + +/* +Convert PWM to DSM. The objective is to convert PWM to a DSM value such that +the inverse conversion done in the Pixhawk results in a different PWM range; +i.e. we are remapping the PWM and converting to DSM at the same time. + +The inverse conversion done on Pixhawk is as follows: + dsm 0..2047 maps to pwm 900..2100 + +src/modules/px4iofirmware/dsm.c, line ~390: + pwm = ((((int)dsm - 1024) * 1000) / 1700) + 1500; + +We want to control that output PWM range as calculated on Pixhawk: + +PWM_in ---> DSM ------> PWM_out + done done on + here Pixhawk + +To do that, we specify a PWM_in range and a PWM_out range, and calculate DSM +values accordingly. + +The approach is to use the PWM_out range to precalculate a DSM range +(inverting the fixed Pixhawk calculation). Each conversion is then just a +linear mapping from the PWM_in range to the DSM range. + +Pixhawk conversion from DSM to PWM: + pwm = ((((int)dsm - 1024) * 1000) / 1700) + 1500; + +Inverse of that: + dsm = (((int)pwm - 1500) * 1700) / 1000 + 1024; + dsm = (1700 * pwm - 1526000) / 1000 + +Using the desired PWM_out range on Pixhawk, calculate DSM_min and DSM_max. + +The the conversion from PWM_in to DSM is: + +dsmRange = dsmMax - dsmMin +pwmInRange = pwmInMax - pwmInMin + +dsm = ((pwmIn - pwmInMin) / pwmInRange) * dsmRange + dsmMin + = ((pwmIn - pwmInMin) * dsmRange + dsmMin * pwmInRange) / pwmInRange + +Adding rounding for the integer math: + +dsm = ((pwmIn - pwmInMin) * dsmRange + dsmMin * pwmInRange + pwmInRange/2) / pwmInRange +*/ + +// This class converts from PWM to DSM values. The "complication" is that we +// want to scale the conversion such that an input PWM range maps to a +// different output PWM range when the DSM->PWM conversion is done on Pixhawk. +// +// There will normally be two of these: one to handle the throttle channel +// (where we want a very specific PWM output range on Pixhawk) and one for all +// other channels (where it will be 1:1). +// +// This does _not_ handle ensuring that a throttle failsafe PWM value of 900 +// maps to a DSM value of zero; in fact it generally will not, and whether it +// does or not depends on the PWM parameters given to the constructor. It is +// expected that whoever calls the 'encode' method will handle the special +// case of throttle failsafe. +class PwmToDsm +{ +public: + // constructor (precalculate some constants) + PwmToDsm(int pwmInMin, int pwmInMax, int pwmOutMin, int pwmOutMax) + : _pwmInMin(pwmInMin), _pwmInMax(pwmInMax), _pwmOutMin(pwmOutMin), _pwmOutMax(pwmOutMax) + { + _pwmInRange = _pwmInMax - _pwmInMin; + + // inverse of what Pixhawk does to convert DSM to PWM (with rounding) + _dsmMin = ((_pwmOutMin * 1700 - 1526000) + 500) / 1000; + _dsmMax = ((_pwmOutMax * 1700 - 1526000) + 500) / 1000; + + _dsmRange = _dsmMax - _dsmMin; + + _k = _dsmMin * _pwmInRange + _pwmInRange / 2; + } + + // convert pwm value to dsm + uint16_t encode(int channel, int pwm) + { + int dsm = ((pwm - _pwmInMin) * _dsmRange + _k) / _pwmInRange; + + // range check + if (dsm < 0) + dsm = 0; + if (dsm > 2047) + dsm = 2047; + + // stuff channel + dsm |= (channel << 11); + + return dsm; + } + + // decode dsm to pwm + // This is only here for diagnostic purposes; + // this is intended to be what the Pixhawk will do + int decode(uint16_t dsm) + { + dsm &= 0x7ff; + return ((((int)dsm - 1024) * 1000) / 1700) + 1500; + } + +private: + // specified values + int _pwmInMin; + int _pwmInMax; + int _pwmOutMin; + int _pwmOutMax; + + // precalculated values + int _pwmInRange; // pwmInMax - pwmInMin + int _dsmMin; // calculated from pwmOutMin + int _dsmMax; // calculated from pwmOutMax + int _dsmRange; // dsmMax - dsmMin + int _k; // (dsmMin * pwmInRange) + (pwmInRange / 2) + +}; // class PwmToDsm + +static PwmToDsm *pwmToDsmThrottle = NULL; +static PwmToDsm *pwmToDsmOther = NULL; + +#if 0 +static void pwmToDsmDiag(PwmToDsm *p2d) +{ + static int pwm_in[] = { 900, 1000, 1100, 1500, 1900, 2000 }; + int pwm_in_max = sizeof(pwm_in) / sizeof(pwm_in[0]); + + for (int i = 0; i < pwm_in_max; i++) + { + uint16_t dsm = p2d->encode(0, pwm_in[i]); + int pwm_out = p2d->decode(dsm); + syslog(LOG_INFO, "main: %4d -> 0x%04x -> %d", pwm_in[i], dsm, pwm_out); + } +} +#endif + +/********************************************************************** +Function: int encode_DSM() + +Description: Encodes channel values (*channels) into a dsm string for + transmission over the DSM serial port. Returns the length + of the string in bytes +***********************************************************************/ +static int encode_DSM(char dsmstr[], uint16_t channels[], int num_channels) +{ + int i; + uint16_t dsm; + uint16_t chNum = 0; + char *str = dsmstr; + + // Encode 16 bytes at a time, with a magic at the start of each + // Empty channels get filled with 0xFF + while (num_channels > 0) { + // The first bytes are identification bytes, we use 0x00AB + *str++ = 0x00; + *str++ = 0xAB; + + // Put 7 channels in the remaining 8 bytes + for (i = 0; i < 7; ++i) { + if (num_channels > 0) { + if (chNum == 0) { + // Handle the special case of throttle failsafe; + // PWM=900 must map to DSM=0 + if (channels[chNum] == 900) + dsm = 0; + else + dsm = pwmToDsmThrottle->encode(chNum, channels[chNum]); + } else { + dsm = pwmToDsmOther->encode(chNum, channels[chNum]); + } + *str++ = dsm >> 8; + *str++ = dsm; + ++chNum; + } + // Empty channels get filled with 0xFF + else { + *str++ = 0xFF; + *str++ = 0xFF; + } + + --num_channels; + } + } + + return (int)(str - dsmstr); +} + +/*********************************************************************** +Function: int serial_setup(void) + +Description: The serial port initialization function. This function + initializes the serial port over which DSM data is sent to + the pixhawk. A return of 0 indicates an error. +***********************************************************************/ +static int serial_setup(void) +{ + struct termios options; + + serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + + if (serial_fd < 0) { + syslog(LOG_ERR, "serial: can't open %s", serialPortName.c_str()); + return 0; + } + + // Configure port for 8N1 transmission + tcgetattr(serial_fd, &options); // Gets the current options for the port + cfsetospeed(&options, B115200); // Sets the Output Baud Rate + + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; + + tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW" + return 1; +} + +/********************************************************************** +Function: void *serial_task(void*) + +Description: The main serial thread task. This function outputs the + latest RC channel data over the DSM serial port, after + converting the RC channel data to DSM format. + +Failsafe detection + +This thread checks to see if the packet in shared memory is changing. +If it does not change within the failsafe timeout, start sending a +failsafe packet until the packet in shared memory starts changing +again. + +Any change in the packet prevents failsafe. Normally the timestamp and +sequence change, but even if those don't change but some channel data +is changing, we don't failsafe (that behavior can be debated). + +Failsafe timeouts are based on the time the packet was fetched from +shared memory. The timestamp in the packet is not used for failsafe +detection. + +Startup - nothing is sent to the Pixhawk until we see the packet in +shared memory change at least once. This is to avoid a startup +condition where this thread runs first (for whatever reason), sees an +unchanging packet in shared memory, and failsafes when we are really +just trying to start everything up. + +***********************************************************************/ +static void *serial_task(void *) +{ + bool started = false; // set true when we see a change in shm + char dsmStr[32]; + uint64_t now_us; + uint64_t last_run_us = 0; + int num_bytes; + void *rc_ipc_id; + struct rc_pkt pkt = {0}; + struct rc_pkt last_pkt = {0}; + unsigned get_err = 0; + uint64_t get_err_time_us = 0; + uint64_t last_good_us = 0; + bool failsafing = false; +#undef DUMP_DSM +#ifdef DUMP_DSM + unsigned decimate = 0; +#endif + + test_bug_exit("/tmp/pixrc_ser_bug"); // simulate crash on startup + + lock_thread_mem(32768); + + // Attach to shared packet memory + rc_ipc_id = rc_ipc_attach(1); + if (rc_ipc_id == NULL) { + syslog(LOG_ERR, "serial: can't attach to rc packet shared memory"); + pthread_exit(NULL); + } + + while (true) { + test_bug_hang("/tmp/pixrc_ser_bug"); // simulate hang + + usleep(SER_PERIOD_US); + + // Get packet from shared memory. This normally does not block, but + // can if someone else is holding the shared memory semaphore. + int get_status = rc_ipc_get(rc_ipc_id, &pkt, 1); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (last_run_us != 0 && ser_run_interval_max_us < (now_us - last_run_us)) + ser_run_interval_max_us = now_us - last_run_us; + last_run_us = now_us; + __atomic_store_n(&ser_run_us, now_us, __ATOMIC_RELAXED); + + if (get_status != 0) { + // Error getting packet from shared memory. + // Failure to get one is probably due to corruption somewhere. + get_err++; + if (now_us - get_err_time_us > 10000000) { + syslog(LOG_ERR, "serial: can't read from rc packet shared memory (count=%d)", + get_err); + get_err_time_us = now_us; + } + // Use the last good packet retrieved. This will be seen as an + // unchanging packet, so if it continues it will cause failsafe. + pkt = last_pkt; + } + + // At this point, pkt is either the packet as retrieved from shared + // memory, or is a copy of the last packet retrieved if we didn't get + // one. + + if (memcmp(&pkt, &last_pkt, sizeof(pkt)) == 0) { + // packet has not changed; check for failsafe timeout + // (we also get here if we could not get one from shared mem) + if ((now_us - last_good_us) > sm_timeout_max_us) { + // packet has not changed for failsafe timeout + pkt.channel[0] = 900; // throttle failsafe value + // center the sticks + pkt.channel[1] = 1500; + pkt.channel[2] = 1500; + pkt.channel[3] = 1500; + // other channels (e.g. gimbal) not touched + + // Note that this is fairly likely to happen the first time + // through the loop if nothing is writing to shared memory + // yet, since the last_good_us is initted to zero, so it will + // look like a timeout. The 'started' flag will be false, so + // this bogus initial failsafe packet will not get sent to + // the Pixhawk. + } + } else { + // packet changed; we got one from shared memory and it is different + // from the previous one + last_good_us = now_us; // used to detect failsafe timeout + last_pkt = pkt; // used to detect changing packet + if (!started) { + syslog(LOG_INFO, "serial: started"); + } + started = true; + } + + // At this point, either the packet changed in shared memory and we're + // using it, or it did not change and (a) it is not too old so we are + // still using it or (b) it has been replaced by the failsafe packet. + + if (started) { + + memset(dsmStr, 0, sizeof(dsmStr)); + num_bytes = encode_DSM(dsmStr, &pkt.channel[0], 8); + + // Check for the rc lockout + if (rc_locked) + continue; + + if (write(serial_fd, dsmStr, num_bytes) != num_bytes) { + syslog(LOG_ERR, "serial: port write error"); + } + + // log failsafe start/stop at the latest possible point (here) + if (failsafing && (dsmStr[2] != 0 || dsmStr[3] != 0)) { + // ending failsafe + syslog(LOG_INFO, "serial: failsafe end"); + failsafing = false; + } else if (!failsafing && (dsmStr[2] == 0 && dsmStr[3] == 0)) { + // starting failsafe + syslog(LOG_INFO, "serial: failsafe start"); + failsafing = true; + } + +#ifdef DUMP_DSM + const unsigned nth = 10; // dump every Nth packet + if (++decimate >= nth) { + for (int i = 0; i < 32; i += 2) { + uint16_t c; + c = (unsigned(dsmStr[i]) << 8) | unsigned(dsmStr[i + 1]); + c &= 0x7ff; + cout << setw(5) << c; + } + cout << endl; + decimate = 0; + } +#endif + + } // if (started) + + } // while + + pthread_exit(NULL); +} + +/********************************************************************** +Function: void start_serial_thread + +Description: Starts the serial thread +***********************************************************************/ +static int start_serial_thread(void) +{ + int ret = 0; + + pthread_create(&serial_ctx, NULL, serial_task, NULL); + + pthread_setname_np(serial_ctx, "pixrc_ser"); + +#ifdef SERIAL_PRIORITY + // Set the priority and scheduler + struct sched_param param; + memset(¶m, 0, sizeof(param)); + param.sched_priority = SERIAL_PRIORITY; + if ((ret = pthread_setschedparam(serial_ctx, SCHED_FIFO, ¶m))) { + syslog(LOG_ERR, "serial: pthread_setschedparam returned %d", ret); + } +#endif // SERIAL_PRIORITY + + return ret; +} + +/********************************************************************** +Function: bool thread_ok + +Description: Returns true if thread appears to be running, false otherwise +***********************************************************************/ +static bool thread_ok(const char *name, volatile uint64_t *thread_us, uint64_t max_us) +{ + + // Ensure now_us is set before reading thread_us. If thread_us were read + // first, a long preemption of this (low priority) thread could cause a + // mistaken timeout event. The __ATOMIC_SEQ_CST in the thread_us read is + // part of this enforcement (prevents code reordering). + + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + + // note thread could run here, making thread_us > now_us + + // get the last time the thread ran + uint64_t last_us = __atomic_load_n(thread_us, __ATOMIC_SEQ_CST); + + if (last_us > now_us) + // thread ran between reading now_us and loading last_us + return true; + + if ((now_us - last_us) <= max_us) + // thread has run recently enough to be ok + return true; + + // too long since thread ran + + // this should be exceptional (bug indicator) + syslog(LOG_ERR, "%s: thread not running", name); + syslog(LOG_ERR, "%s: now=%llu, ran@%llu, delta=%llu", name, now_us, last_us, now_us - last_us); + + return false; + +} // thread_ok + +/********************************************************************** +Function: bool thread_started + +Description: Returns true if thread appears to be running, false + otherwise. + +This waits up to a timeout to let the thread get started. + +A thread is deemed "running" if its last activation time is nonzero. +This depends on the activation time being statically initialized to +zero, but means we don't have to wait for an entire thread activation +period to know it is running. For example, the logging task may only +run once per second, so we don't really want to wait for its activation +time to change. +***********************************************************************/ +static bool thread_started(const char *name, volatile uint64_t *thread_us, uint64_t timeout_us) +{ + + // Verify thread_us becomes nonzero, waiting up to timeout_us. + + // This is done at startup to make sure the thread is running, and + // to know that we can start monitoring thread_us to detect thread + // stoppage. + + uint64_t start_us = clock_gettime_us(CLOCK_MONOTONIC); + uint64_t now_us = start_us; + + while (now_us - start_us <= timeout_us) { + if (__atomic_load_n(thread_us, __ATOMIC_SEQ_CST) != 0) + return true; + usleep(1000); + now_us = clock_gettime_us(CLOCK_MONOTONIC); + } + + // at this point it has been 1 usleep since we last checked + if (__atomic_load_n(thread_us, __ATOMIC_SEQ_CST) != 0) + return true; + + // this should be exceptional (bug indicator) + syslog(LOG_ERR, "%s: thread failed to start", name); + + return false; + +} // thread_started + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(void) +{ + string ipstr; + + openlog("pixrc", LOG_NDELAY, LOG_LOCAL0); + + syslog(LOG_INFO, "main: built " __DATE__ " " __TIME__); + + /* Parse the sololink.conf file for serial port, source IPs and ports */ + INIReader reader("/etc/sololink.conf"); + + if (reader.ParseError() < 0) { + std::cout << "Can't load /etc/sololink.conf\n"; + return -1; + } + + /* Generate a vector of IP addresses that we can accept RC data from */ + string rcSourceIps = reader.Get("solo", "rcSourceIps", "10.1.1.1"); + istringstream ss(rcSourceIps); + while (getline(ss, ipstr, ',')) + sourceIPs.push_back(ipstr); + + /* The incoming UDP port from which we accept data */ + sourcePort = reader.GetInteger("solo", "rcDestPort", 5005); + + /* Get the serial port to output DSM data on */ + serialPortName = reader.Get("solo", "rcDsmDev", "/dev/ttymxc2"); + + // PWM mappings + // throttle channel + int pwmInMinThrottle = reader.GetInteger("solo", "pwmInMinThrottle", 1000); + int pwmInMaxThrottle = reader.GetInteger("solo", "pwmInMaxThrottle", 2000); + int pwmOutMinThrottle = reader.GetInteger("solo", "pwmOutMinThrottle", 1000); + int pwmOutMaxThrottle = reader.GetInteger("solo", "pwmOutMaxThrottle", 1900); + // all other channels + int pwmInMinOther = reader.GetInteger("solo", "pwmInMinOther", 1000); + int pwmInMaxOther = reader.GetInteger("solo", "pwmInMaxOther", 2000); + int pwmOutMinOther = reader.GetInteger("solo", "pwmOutMinOther", 1000); + int pwmOutMaxOther = reader.GetInteger("solo", "pwmOutMaxOther", 2000); + + /* RC timeout in microseconds */ + rc_timeout_max_us = reader.GetInteger("solo", "rcTimeoutUS", RC_TIMEOUT_US); + + /* Shotmgr timeout in microseconds */ + sm_timeout_max_us = reader.GetInteger("solo", "smTimeoutUS", SM_TIMEOUT_US); + + // create the mappers + pwmToDsmThrottle = new (std::nothrow) + PwmToDsm(pwmInMinThrottle, pwmInMaxThrottle, pwmOutMinThrottle, pwmOutMaxThrottle); + if (pwmToDsmThrottle == NULL) { + syslog(LOG_ERR, "main: can't create pwmToDsmThrottle"); + return -1; + } + pwmToDsmOther = + new (std::nothrow) PwmToDsm(pwmInMinOther, pwmInMaxOther, pwmOutMinOther, pwmOutMaxOther); + if (pwmToDsmOther == NULL) { + syslog(LOG_ERR, "main: can't create pwmToDsmOther"); + return -1; + } + + syslog(LOG_INFO, "main: rcSourceIps: %s", rcSourceIps.c_str()); + syslog(LOG_INFO, "main: rcDestPort: %d", sourcePort); + syslog(LOG_INFO, "main: rcDsmDev: %s", serialPortName.c_str()); + syslog(LOG_INFO, "main: Throttle PWM in %4d...%4d", pwmInMinThrottle, pwmInMaxThrottle); + syslog(LOG_INFO, "main: PWM out %4d...%4d", pwmOutMinThrottle, pwmOutMaxThrottle); + // pwmToDsmDiag(pwmToDsmThrottle); + syslog(LOG_INFO, "main: Other PWM in %4d...%4d", pwmInMinOther, pwmInMaxOther); + syslog(LOG_INFO, "main: PWM out %4d...%4d", pwmOutMinOther, pwmOutMaxOther); + // pwmToDsmDiag(pwmToDsmOther); + + /* Set up the UDP and serial ports */ + if (!UDP_setup()) { + // specific error already logged + return -1; + } + + if (!serial_setup()) { + // specific error already logged + return -1; + } + + // Start the command processor + command = new RcCommander("/run/rc_uplink_cmd"); + + rc_locked = RcLock::locked(); + + // Start the UDP and serial threads. UDP is started first so if the uplink + // is already streaming, we won't get an initial throttle failsafe. + start_UDP_thread(); + usleep(100000); // 0.1 sec + start_serial_thread(); + + // Wait for all to be running so we can't possibly detect a dead thread on + // the first check below. It would be possible to guarantee they run first + // (based on priorities) but that would be a fragile dependence. + if (!thread_started("udp", &udp_run_us, UDP_STUCK_US) || + !thread_started("serial", &ser_run_us, SER_STUCK_US)) { + // thread_started logs the error + exit(0); + } + + // The main while() loop just becomes a thread monitor. If any thread dies, + // exit and let inittab restart us + while (true) { + rc_locked = RcLock::locked(); + + // Check that all threads are running (and not stuck). + + if (!thread_ok("udp", &udp_run_us, UDP_STUCK_US) || + !thread_ok("serial", &ser_run_us, SER_STUCK_US)) { + // thread_ok logs the error + exit(0); + } + + usleep(100000); // check every 0.1 sec + } + + return -1; + +} // main diff --git a/flightcode/pixrc/rc_ipc.c b/flightcode/pixrc/rc_ipc.c new file mode 100644 index 0000000..abb01c0 --- /dev/null +++ b/flightcode/pixrc/rc_ipc.c @@ -0,0 +1,196 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "rc_pkt.h" +#include "rc_ipc.h" + +/* these must start with slash then contain no more slashes */ +#define RC_IPC_SHM_NAME "/rc_shm" +#define RC_IPC_SEM_NAME "/rc_sem" + +/* + * Information about an instance of the RC IPC. One of these is allocated and + * returned from create or attach, and passed back to the other functions. An + * application must eventually give it to detach/delete to free it. + */ +struct rc_ipc_info { + uint32_t magic; + struct rc_pkt *pkt; + sem_t *sem; +}; + +#define RC_IPC_INFO_MAGIC 0x21436587 + +/* return rc_ipc_id pointer on success, NULL on error */ +void *rc_ipc_attach(int verbosity) +{ + struct rc_ipc_info *info; + int fd; + + info = (struct rc_ipc_info *)malloc(sizeof(struct rc_ipc_info)); + if (info == NULL) { + if (verbosity > 0) + perror("malloc"); + return NULL; + } + + memset(info, 0, sizeof(struct rc_ipc_info)); + + /* create or attach shared memory */ + + fd = shm_open(RC_IPC_SHM_NAME, O_RDWR | O_CREAT, 0666); + if (fd < 0) { + if (verbosity > 0) + perror("shm_open"); + free(info); + return NULL; + } + + if (ftruncate(fd, sizeof(struct rc_pkt)) != 0) { + if (verbosity > 0) + perror("ftruncate"); + close(fd); /* instead of shm_unlink() */ + free(info); + return NULL; + } + + info->pkt = mmap(NULL, sizeof(struct rc_pkt), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + if (info->pkt == MAP_FAILED) { + if (verbosity > 0) + perror("mmap"); + close(fd); + free(info); + return NULL; + } + + /* after mapping, the fd is no longer needed */ + close(fd); + + /* create semaphore */ + + info->sem = sem_open(RC_IPC_SEM_NAME, O_CREAT, 0666, 1); + if (info->sem == SEM_FAILED) { + if (verbosity > 0) + perror("sem_open"); + munmap(info->pkt, sizeof(struct rc_pkt)); + free(info); + return NULL; + } + + info->magic = RC_IPC_INFO_MAGIC; + + if (verbosity > 1) + printf("%s: shared memory and semaphore created\n", __FUNCTION__); + + return (void *)info; + +} /* rc_ipc_attach */ + +/* return 0 on success, nonzero on error */ +int rc_ipc_put(void *rc_ipc_id, const struct rc_pkt *pkt, int verbosity) +{ + struct rc_ipc_info *info = (struct rc_ipc_info *)rc_ipc_id; + + /* sanity check */ + if (info->magic != RC_IPC_INFO_MAGIC) { + printf("%s: ERROR: bad magic\n", __FUNCTION__); + return -1; + } + + /* lock */ + if (sem_wait(info->sem) != 0) { + if (verbosity > 0) + perror("sem_wait"); + return -1; + } + + /* write packet */ + memcpy(info->pkt, pkt, sizeof(struct rc_pkt)); + + /* unlock */ + if (sem_post(info->sem) != 0) { + if (verbosity > 0) + perror("sem_post"); + return -1; + } + + if (verbosity > 1) + printf("%s: packet written to shared memory\n", __FUNCTION__); + + return 0; + +} /* rc_ipc_put */ + +/* return 0 on success, nonzero on error */ +int rc_ipc_get(void *rc_ipc_id, struct rc_pkt *pkt, int verbosity) +{ + struct rc_ipc_info *info = (struct rc_ipc_info *)rc_ipc_id; + + /* sanity check */ + if (info->magic != RC_IPC_INFO_MAGIC) { + printf("%s: ERROR: bad magic\n", __FUNCTION__); + return -1; + } + + /* lock */ + if (sem_wait(info->sem) != 0) { + if (verbosity > 0) + perror("sem_wait"); + return -1; + } + + /* read packet */ + memcpy(pkt, info->pkt, sizeof(struct rc_pkt)); + + /* unlock */ + if (sem_post(info->sem) != 0) { + if (verbosity > 0) + perror("sem_post"); + return -1; + } + + if (verbosity > 1) + printf("%s: packet read from shared memory\n", __FUNCTION__); + + return 0; + +} /* rc_ipc_get */ + +/* return 0 on success, nonzero on error */ +int rc_ipc_detach(void *rc_ipc_id, int verbosity) +{ + struct rc_ipc_info *info = (struct rc_ipc_info *)rc_ipc_id; + int status = 0; + + /* sanity check */ + if (info->magic != RC_IPC_INFO_MAGIC) { + printf("%s: ERROR: bad magic\n", __FUNCTION__); + return -1; + } + + info->magic = 0; + + if (munmap(info->pkt, sizeof(struct rc_pkt)) != 0) { + perror("munmap"); + status = -1; + } + info->pkt = NULL; + + if (sem_close(info->sem) != 0) { + perror("sem_close"); + status = -1; + } + info->sem = NULL; + + free(info); + + return status; + +} /* rc_ipc_detach */ diff --git a/flightcode/pixrc/rc_ipc.h b/flightcode/pixrc/rc_ipc.h new file mode 100644 index 0000000..b9145c2 --- /dev/null +++ b/flightcode/pixrc/rc_ipc.h @@ -0,0 +1,28 @@ +#ifndef RC_IPC_H +#define RC_IPC_H + +/* + * Definitions supporting IPC sharing of RC packets. Internally, there + * is a shared memory containing an RC packet (rc_pkt.h) and a semaphore + * controlling access. + */ + +#include "rc_pkt.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern void *rc_ipc_attach(int verbosity); + +extern int rc_ipc_put(void *rc_ipc_id, const struct rc_pkt *pkt, int verbosity); + +extern int rc_ipc_get(void *rc_ipc_id, struct rc_pkt *pkt, int verbosity); + +extern int rc_ipc_detach(void *rc_ipc_id, int verbosity); + +#ifdef __cplusplus +}; +#endif + +#endif /* RC_IPC_H */ diff --git a/flightcode/pixrc/rc_pkt.h b/flightcode/pixrc/rc_pkt.h new file mode 100644 index 0000000..65837e4 --- /dev/null +++ b/flightcode/pixrc/rc_pkt.h @@ -0,0 +1,30 @@ +#ifndef RC_PKT_H +#define RC_PKT_H + +/* + * This is the RC packet structure as found in shared memory on Solo. + * At startup, it is written by the pixrc process, UDP_task. At runtime, it + * may be switched to be updated by some other flight-control process. It is + * always read by the pixrc process, serial_task, which converts it to DSM and + * writes it to the Pixhawk. + */ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define NUM_CHANNELS 8 + +struct __attribute((__packed__)) rc_pkt { + uint64_t timestamp; + uint16_t sequence; + uint16_t channel[NUM_CHANNELS]; +}; + +#ifdef __cplusplus +}; +#endif + +#endif /* RC_PKT_H */ diff --git a/flightcode/pixrc/test_pixrc.cpp b/flightcode/pixrc/test_pixrc.cpp new file mode 100644 index 0000000..f0666ba --- /dev/null +++ b/flightcode/pixrc/test_pixrc.cpp @@ -0,0 +1,87 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" + +using namespace std; + +/*********************************************************************** +UDP macros +***********************************************************************/ +#define UDP_PORT 5005 +#define NUM_CHANNELS 8 + +int main(void) +{ + int sock_fd; + struct sockaddr_in remaddr; /* server address */ + int slen = sizeof(remaddr); + char *server = "10.1.1.10"; + // char *server = "127.0.0.1"; /* change this to use a different server */ + uint16_t channelVals[NUM_CHANNELS] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; + uint16_t sequence = 0; + uint64_t timestamp = 0; + uint64_t last_us, now_us; + char buf[512]; + int delta; + struct sched_param param; + + srand(time(NULL)); + + if ((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + cerr << "cannot create socket" << endl; + return 0; + } + + int tos_local = 0xFF; + setsockopt(sock_fd, IPPROTO_IP, IP_TOS, &tos_local, sizeof(tos_local)); + + memset((char *)&remaddr, 0, sizeof(remaddr)); + remaddr.sin_family = AF_INET; + remaddr.sin_port = htons(UDP_PORT); + if (inet_aton(server, &remaddr.sin_addr) == 0) { + cerr << "inet_aton() failed" << endl; + return 0; + } + + memset(¶m, 0, sizeof(param)); + param.sched_priority = sched_get_priority_max(SCHED_FIFO); + pthread_setschedparam(0, SCHED_FIFO, ¶m); + + last_us = clock_gettime_us(CLOCK_MONOTONIC); + + while (true) { + now_us = clock_gettime_us(CLOCK_MONOTONIC); + delta = now_us - last_us; + if (delta > 21000) + cout << "Slow! " << delta << "us" << endl; + last_us = now_us; + memset(buf, 0, sizeof(buf)); + memcpy(buf, ×tamp, 8); + memcpy(&buf[8], &sequence, 2); + memcpy(&buf[10], channelVals, 16); + + /* now let's send the messages */ + if (sendto(sock_fd, buf, 26, 0, (struct sockaddr *)&remaddr, slen) == -1) { + cerr << "sendto failed" << endl; + return 0; + } + + timestamp += 1; + sequence += 1; + + uint64_t delay = 20000; // + (rand() % 10000 - 5000); + usleep(delay); + } + + close(sock_fd); + return 1; +} diff --git a/flightcode/proc_top/Makefile b/flightcode/proc_top/Makefile new file mode 100644 index 0000000..d7cf20e --- /dev/null +++ b/flightcode/proc_top/Makefile @@ -0,0 +1,40 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) + +SRCS_C = main.c proc_table.c util.c + +OBJS = $(SRCS_C:.c=.o) + +MAIN = proc_top + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.c) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/proc_top/main.c b/flightcode/proc_top/main.c new file mode 100644 index 0000000..01dba68 --- /dev/null +++ b/flightcode/proc_top/main.c @@ -0,0 +1,117 @@ + +#include +#include +#include +#include "util.h" +#include "proc_table.h" + +#define MAX_PROC_ENTRIES 100 +static proc_entry_t proc_table_1[MAX_PROC_ENTRIES]; +static proc_entry_t proc_table_2[MAX_PROC_ENTRIES]; +static proc_entry_t proc_table_3[MAX_PROC_ENTRIES]; + +int main(int argc, char *argv[]) +{ + FILE *fp = stdout; + int proc_entries_1; + int proc_entries_2; + int proc_entries_3; + int i; + proc_entry_t *pt1; + proc_entry_t *pt2; + proc_entry_t *ptx; + int *pe1; + int *pe2; + int *pex; + uint64_t now_us; + uint64_t next_us; + long unsigned percent; + char time_buf[32]; + long unsigned sum; + const unsigned interval_us = 10000000; + const unsigned hz = 100; + const unsigned interval_jiffies = (interval_us * hz) / 1000000; + + /* one argument; if supplied, it's the output filename */ + if (argc > 1) { + FILE *fp2 = fopen(argv[1], "a"); + if (fp2 != NULL) + fp = fp2; + } + + now_us = clock_gettime_us(CLOCK_REALTIME); + fprintf(fp, "\n"); + fprintf(fp, "%s proc_top starting\n", clock_tostr_r(now_us, time_buf)); + + next_us = clock_gettime_us(CLOCK_MONOTONIC) + interval_us; + + proc_entries_1 = MAX_PROC_ENTRIES; + if (proc_table_get(proc_table_1, &proc_entries_1) != 0) { + fprintf(stderr, "ERROR reading proc table\n"); + exit(1); + } + + pt1 = proc_table_1; + pt2 = proc_table_2; + pe1 = &proc_entries_1; + pe2 = &proc_entries_2; + + while (1) { + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (next_us > now_us) + usleep(next_us - now_us); + next_us += interval_us; + + now_us = clock_gettime_us(CLOCK_REALTIME); + + *pe2 = MAX_PROC_ENTRIES; + if (proc_table_get(pt2, pe2) != 0) { + fprintf(stderr, "ERROR reading proc table\n"); + exit(1); + } + + proc_entries_3 = MAX_PROC_ENTRIES; + proc_table_diff(pt1, *pe1, pt2, *pe2, proc_table_3, &proc_entries_3); + + proc_table_top(proc_table_3, proc_entries_3); + + fprintf(fp, "\n"); + fprintf(fp, "%s\n", clock_tostr_r(now_us, time_buf)); + fprintf(fp, "%6s %-20s %6s %6s %4s\n", "pid", "name", "utime", "stime", "pct"); + for (i = 0; i < proc_entries_3; i++) { +#if 0 + /* Quit printing when the total time becomes zero. This prints + a lot of nearly-idle tasks. */ + if (proc_table_3[i].utime == 0 && proc_table_3[i].stime == 0) + break; +#endif + sum = proc_table_3[i].utime + proc_table_3[i].stime; + percent = (100 * sum + interval_jiffies / 2) / interval_jiffies; +#if 1 + /* Quit printing when the the percentage rounds to zero. The + CPU usage is *not* zero, but this gives us a handful (~5) + of tasks per dump. */ + if (percent == 0) + break; +#endif + fprintf(fp, "%6d %-20s %6lu %6lu %3lu%%\n", proc_table_3[i].pid, proc_table_3[i].comm, + proc_table_3[i].utime, proc_table_3[i].stime, percent); + } + + fflush(fp); + + /* swap tables */ + + ptx = pt1; + pt1 = pt2; + pt2 = ptx; + + pex = pe1; + pe1 = pe2; + pe2 = pex; + + } /* while (1) */ + + exit(0); + +} /* main */ diff --git a/flightcode/python/app_server.py b/flightcode/python/app_server.py new file mode 100755 index 0000000..a8f34b2 --- /dev/null +++ b/flightcode/python/app_server.py @@ -0,0 +1,186 @@ +#!/usr/bin/python + +# This uses the above '#!' instead of '#!/usr/bin/env python' so that the +# busybox 'pidof' can find this process by name. + +import ConfigParser +import logging +import logging.config +import optparse +import os +import socket +import struct +import sys +sys.path.append("/usr/bin") +import app_connected_msg + +solo_conf = "/etc/sololink.conf" + +# items read from solo_conf +app_server_port = 0 +app_address_file = "" + + + +def get_request(sock): + """get one request from socket + + A request is a 32-bit byte count followed by opaque data. The byte count + includes itself, i.e. the minimum byte count is four. The request is + returned as a string, with the initial byte count still included. The + purpose of "requests" at this level is really to just provide for reliable + datagrams over TCP. + """ + + # Read byte count + # XXX This is inefficient but simple; we never want to read beyond the end + # of the request we are receiving, to avoid needing to save state between + # calls to get_request. + pkt = "" + while len(pkt) < 4: + try: + b = sock.recv(1) + except socket.error as se: + # Android app: disconnect wifi without closing app and you get: + # socket.error: [Errno 110] Connection timed out + logger.info("socket.error: %s", str(se)) + return None + except socket.timeout as st: + # Has not been observed to happen. + logger.info("socket.timeout: %s", str(st)) + return None + if not b: + return None + pkt += b + + (pkt_len, ) = struct.unpack("!I", pkt) + + logger.debug("packet length %d", pkt_len) + + # Read the rest of the packet + # XXX Super-inefficient. Don't read beyond the end of the current packet. + while len(pkt) < pkt_len: + try: + b = sock.recv(1) + except socket.error as se: + logger.info("socket.error: %s", str(se)) + return None + except socket.timeout as st: + logger.info("socket.timeout: %s", str(st)) + return None + if not b: + return None + pkt += b + + return pkt + + + +def set_app_ip(app_ip): + f = open(app_address_file, "w") + f.write(app_ip + "\n") + f.close() + + + +def unset_app_ip(): + # allow it to not exist (unlink fails) + try: + os.unlink(app_address_file) + except: + pass + + + +# app server connection loop +def app_server(): + + # socket we will use to listen for connections + listen_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + listen_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + listen_sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) + # After 1 second, start KEEPALIVE + listen_sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 1) + # 5 seconds in between keepalive pings + listen_sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 5) + # 5 max fails + listen_sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 5) + listen_sock.bind(("", app_server_port)) + + # only allow one connection + listen_sock.listen(0) + + while True: + + logger.info("waiting for connection") + + # wait for connection + (sock, address) = listen_sock.accept() + + logger.info("connection from %s", str(address)) + + set_app_ip(address[0]) + logger.info("app IP is %s", address[0]) + + app_connected_msg.send_connected() + + # process requests from the client + while True: + # Get one request. We don't yet use these; it is the existence of + # the connection that is meaningful. + pkt = get_request(sock) + if not pkt: + # Remote end closed the connection + break + logger.info("received request: %s", str([hex(ord(x)) for x in pkt])) + ### end while True + + app_connected_msg.send_disconnected() + + unset_app_ip() + + logger.info("closing connection") + try: + sock.shutdown(socket.SHUT_RDWR) + except socket.error as se: + # Android app: disconnect wifi without closing app and you get: + # socket.error: [Errno 107] Transport endpoint is not connected + logger.info("socket.error: %s", str(se)) + except socket.timeout as st: + # Has not been observed to happen. + logger.info("socket.timeout: %s", str(st)) + sock.close() + + ### end while True + +### end app_server() + + + +if __name__ == "__main__": + + logging.config.fileConfig(solo_conf) + logger = logging.getLogger("app") + + logger.info("starting") + + config = ConfigParser.SafeConfigParser() + + # if the config file is not found, and empty list is returned and the + # "get" operations later fail + config.read(solo_conf) + + # read configuration items + try: + app_server_port = config.getint("solo", "appServerPort") + app_address_file = config.get("solo", "appAddressFile") + except: + logger.error("error reading config from %s", solo_conf) + sys.exit(1) + + parser = optparse.OptionParser("app_server [options]") + + (opts, args) = parser.parse_args() + + app_server() + # app_server never returns diff --git a/flightcode/python/ath9k.py b/flightcode/python/ath9k.py new file mode 100755 index 0000000..eeef79d --- /dev/null +++ b/flightcode/python/ath9k.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +import re +import sys +import time +#sys.path.append("/usr/bin") +#import clock + + +# This takes ~20 msec +# (VO): qnum: 3 qdepth: 0 ampdu-depth: 0 pending: 0 stopped: 0 +r = re.compile(\ +"\ +\((.+)\):\ + +([a-z-]+): +([0-9]+)\ + +([a-z-]+): +([0-9]+)\ + +([a-z-]+): +([0-9]+)\ + +([a-z-]+): +([0-9]+)\ + +([a-z-]+): +([0-9]+)\ +") + + +def get_queues(): + try: + f = open("/sys/kernel/debug/ieee80211/phy0/ath9k/queues") + except: + return None + s = f.read() + f.close() + s = s.splitlines() + d = { } + for line in s: + m = r.match(line) + v = { } + v[m.group(2)] = int(m.group(3)) + v[m.group(4)] = int(m.group(5)) + v[m.group(6)] = int(m.group(7)) + v[m.group(8)] = int(m.group(9)) + v[m.group(10)] = int(m.group(11)) + d[m.group(1)] = v + return d + + +graph = True + +if __name__ == "__main__": + last = { } + count = 0 + while True: + qs = get_queues() + if graph: + if count == 0: + line = ['-'] * 128 + count = 9 + else: + line = [' '] * 128 + count -= 1 + for c in range(7): + line[c * 20] = '|' + for q in qs: + val = qs[q]['pending'] + if val > 127: + val = 127 + #print q, qs[q], str(qs[q]), str(qs[q])[1] + line[val] = q[1] + print "".join(line) + else: + for q in qs: + # if too many packets pending twice in a row, print message + if q in last and last[q] > 120 and qs[q]['pending'] > 120: + print q, last[q], qs[q]['pending'] + last[q] = qs[q]['pending'] + time.sleep(0.1) diff --git a/flightcode/python/attic/checkPixBaud.py b/flightcode/python/attic/checkPixBaud.py new file mode 100755 index 0000000..1ffedf6 --- /dev/null +++ b/flightcode/python/attic/checkPixBaud.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python + +import subprocess +import sys +import os +import time +from pymavlink import mavutil +import glob +import ConfigParser +import shutil +from datetime import datetime +import re +import json +import argparse + +SELECT_GPIO = "21" +ENABLE_GPIO = "19" + +sololink_conf = "/etc/sololink.conf" + +#GPIO direction set +def setGPIODir(gpio, direction): + dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w") + dir_fd.write(direction) + dir_fd.close() + +#Open the GPIO +def openGPIO(gpio): + #Check and see if the GPIO is already exported + if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)): + #otherwise export it + exp_fd = open("/sys/class/gpio/export", "w") + exp_fd.write(gpio) + exp_fd.close() + + setGPIODir(gpio, "out"); + +def closeGPIO(gpio): + unexp_fd = open("/sys/class/gpio/unexport", "w") + unexp_fd.write(gpio) + unexp_fd.close() + +def setGPIO(gpio, value): + val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w") + val_fd.write(value) + val_fd.close() + +def openSetClose(gpio, value): + openGPIO(gpio) + setGPIO(gpio, value) + closeGPIO(gpio) + +def disconnectAndExit(): + openSetClose(SELECT_GPIO, "0") + openSetClose(ENABLE_GPIO, "1") + sys.exit() + +#Bootloading process +print "Pixhawk telem baudrate checker" + +config = ConfigParser.SafeConfigParser() +config.read(sololink_conf) +telemBaud = config.getint("solo","telemBaud") + +#Try to connect over the telem and get a heartbeat +m = mavutil.mavlink_connection("/dev/ttymxc1", baud=telemBaud) +m.set_rtscts(True) + +hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=1) +m.close() +if not hb: + print "Did not receieve a heartbeat on telem. Checking USB." +else: + print "Got a heartbeat, this baudrate is correct." + disconnectAndExit(); + +#Set the USB select GPIOs +openSetClose(SELECT_GPIO, "1") +openSetClose(ENABLE_GPIO, "0") +time.sleep(1) + +#Try to get a heartbeat on the USB. +usb_devs = glob.glob('/dev/serial/by-id/usb-3D*') +if not usb_devs: + print "No pixhawk found on USB. Exiting." + disconnectAndExit() + +print "Pixhawk found on USB, requesting SERIAL1_BAUD" +pixhawk_usb = usb_devs[-1] +m = mavutil.mavlink_connection(pixhawk_usb) + +m.param_fetch_one("SERIAL1_BAUD") +msg = m.recv_match(type='PARAM_VALUE', blocking=True, timeout=3) +if not msg: + print "Did not get param back!" + m.close() + disconnectAndExit() +if msg: + print "Got value:" + str(msg.param_value) + + if(msg.param_value == 1.): + baud_setting = 1200 + elif(msg.param_value == 2.): + baud_setting = 2400 + elif(msg.param_value == 4.): + baud_setting = 4800 + elif(msg.param_value == 9.): + baud_setting = 9600 + elif(msg.param_value == 19.): + baud_setting = 19200 + elif(msg.param_value == 38.): + baud_setting = 38400 + elif(msg.param_value == 57.): + baud_setting = 57600 + elif(msg.param_value == 111.): + baud_setting = 111100 + elif(msg.param_value == 115.): + baud_setting = 115200 + elif(msg.param_value == 500.): + baud_setting = 500000 + elif(msg.param_value == 921.): + baud_setting = 921600 + elif(msg.param_value == 1500.): + baud_setting = 1500000 + else: + print "Unknown baudrate!" + m.close() + disconnectAndExit() + + print "Setting telemBaud to " + str(baud_setting) + os.system("sed -i \"s/telemBaud.*=.*/telemBaud = "+str(baud_setting)+"/\" /etc/sololink.conf") + os.system("md5sum /etc/sololink.conf > /etc/sololink.conf.md5") + +m.close() + +disconnectAndExit() diff --git a/flightcode/python/gpio.py b/flightcode/python/gpio.py new file mode 100644 index 0000000..8f50287 --- /dev/null +++ b/flightcode/python/gpio.py @@ -0,0 +1,48 @@ +import os + + +def export(gpio): + # is GPIO already exported? + if os.path.isdir("/sys/class/gpio/gpio" + str(gpio)): + return + # no, export it + f = open("/sys/class/gpio/export", "w") + f.write(str(gpio)) + f.close() + + +def unexport(gpio): + f = open("/sys/class/gpio/unexport", "w") + f.write(str(gpio)) + f.close() + + +# set direction +# gpio is a string or integer (e.g. "21" or 21) +# direction is "in" or "out" +def set_dir(gpio, direction): + filename = "/sys/class/gpio/gpio%s/direction" % str(gpio) + try: + f = open(filename, "w") + except: + print "error opening %s (need to export first?)" % filename + return + f.write(direction) + f.close() + + +# set output value +# gpio is a string or integer (e.g. "21" or 21) +# value is a string or integer ("0", "1", 0, 1) +def set(gpio, value): + filename = "/sys/class/gpio/gpio%s/value" % str(gpio) + try: + f = open(filename, "w") + except: + print "error opening %s (need to export first?)" % filename + return + try: + f.write(str(value)) + f.close() + except: + print "error setting %s (is it an output?)" % filename diff --git a/flightcode/python/led_nop.py b/flightcode/python/led_nop.py new file mode 100644 index 0000000..7470e37 --- /dev/null +++ b/flightcode/python/led_nop.py @@ -0,0 +1,6 @@ + +def blink(on_ms, off_ms): + pass + +def off(): + pass diff --git a/flightcode/python/led_solo.py b/flightcode/python/led_solo.py new file mode 100644 index 0000000..2631983 --- /dev/null +++ b/flightcode/python/led_solo.py @@ -0,0 +1,9 @@ +import os + +def blink(on_ms, off_ms): + os.system("echo timer > /sys/class/leds/user2/trigger") + os.system("echo %d > /sys/class/leds/user2/delay_on" % (on_ms, )) + os.system("echo %d > /sys/class/leds/user2/delay_off" % (off_ms, )) + +def off(): + os.system("echo none > /sys/class/leds/user2/trigger") diff --git a/flightcode/python/lsproc.py b/flightcode/python/lsproc.py new file mode 100755 index 0000000..48033c5 --- /dev/null +++ b/flightcode/python/lsproc.py @@ -0,0 +1,329 @@ +#!/usr/bin/env python + +import os +import re + + +# This was created to list all processes with their real-time priorities. +# If there is a standard way to do that, that would be preferred. + + +def get_task_stats(pid, tid): + try: + f = open("/proc/%s/task/%s/stat" % (str(pid), str(tid))) + except: + return None + s = f.read() + f.close() + return s + + +def get_all_task_stats(): + p = [] + for f in os.listdir("/proc"): + try: + pid = int(f) + except: + continue + for g in os.listdir("/proc/%d/task" % pid): + try: + tid = int(g) + except: + continue + s = get_task_stats(pid, tid) + p.append(s) + ### for g + ### for f + return p + + +def print_all_task_stats(ps): + print "%5s %-15s %4s %6s %6s" % ("pid", "name", "prio", "utime", "stime") + for p in ps: + if not p: + continue + m = re.match("([0-9]+) \((.*?)\) (.) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+) ([0-9\-]+)", p) + if not m: + continue + print "%5s %-15s %4s %6s %6s" % (m.group(1), m.group(2), m.group(18), m.group(14), m.group(15)) + + +if __name__ == "__main__": + ps = get_all_task_stats() + print_all_task_stats(ps) + + +# /proc/[pid]/stat +# Status information about the process. This is used by ps(1). +# It is defined in the kernel source file fs/proc/array.c. +# +# The fields, in order, with their proper scanf(3) format +# specifiers, are: +# +# (1) pid %d +# The process ID. +# +# (2) comm %s +# The filename of the executable, in parentheses. +# This is visible whether or not the executable is +# swapped out. +# +# (3) state %c +# One of the following characters, indicating process +# state: +# R Running +# S Sleeping in an interruptible wait +# D Waiting in uninterruptible disk sleep +# Z Zombie +# T Stopped (on a signal) or (before Linux 2.6.33) +# trace stopped +# t Tracing stop (Linux 2.6.33 onward) +# W Paging (only before Linux 2.6.0) +# X Dead (from Linux 2.6.0 onward) +# x Dead (Linux 2.6.33 to 3.13 only) +# K Wakekill (Linux 2.6.33 to 3.13 only) +# W Waking (Linux 2.6.33 to 3.13 only) +# P Parked (Linux 3.9 to 3.13 only) +# +# (4) ppid %d +# The PID of the parent of this process. +# +# (5) pgrp %d +# The process group ID of the process. +# +# (6) session %d +# The session ID of the process. +# +# (7) tty_nr %d +# The controlling terminal of the process. (The minor +# device number is contained in the combination of +# bits 31 to 20 and 7 to 0; the major device number is +# in bits 15 to 8.) +# +# (8) tpgid %d +# The ID of the foreground process group of the +# controlling terminal of the process. +# +# (9) flags %u +# The kernel flags word of the process. For bit +# meanings, see the PF_* defines in the Linux kernel +# source file include/linux/sched.h. Details depend +# on the kernel version. +# +# The format for this field was %lu before Linux 2.6. +# +# (1) minflt %lu +# The number of minor faults the process has made +# which have not required loading a memory page from +# disk. +# +# (11) cminflt %lu +# The number of minor faults that the process's +# waited-for children have made. +# +# (12) majflt %lu +# The number of major faults the process has made +# which have required loading a memory page from disk. +# +# (13) cmajflt %lu +# The number of major faults that the process's +# waited-for children have made. +# +# (14) utime %lu +# Amount of time that this process has been scheduled +# in user mode, measured in clock ticks (divide by +# sysconf(_SC_CLK_TCK)). This includes guest time, +# guest_time (time spent running a virtual CPU, see +# below), so that applications that are not aware of +# the guest time field do not lose that time from +# their calculations. +# +# (15) stime %lu +# Amount of time that this process has been scheduled +# in kernel mode, measured in clock ticks (divide by +# sysconf(_SC_CLK_TCK)). +# +# (16) cutime %ld +# Amount of time that this process's waited-for +# children have been scheduled in user mode, measured +# in clock ticks (divide by sysconf(_SC_CLK_TCK)). +# (See also times(2).) This includes guest time, +# cguest_time (time spent running a virtual CPU, see +# below). +# +# (17) cstime %ld +# Amount of time that this process's waited-for +# children have been scheduled in kernel mode, +# measured in clock ticks (divide by +# sysconf(_SC_CLK_TCK)). +# +# (18) priority %ld +# (Explanation for Linux 2.6) For processes running a +# real-time scheduling policy (policy below; see +# sched_setscheduler(2)), this is the negated +# scheduling priority, minus one; that is, a number in +# the range -2 to -100, corresponding to real-time +# priorities 1 to 99. For processes running under a +# non-real-time scheduling policy, this is the raw +# nice value (setpriority(2)) as represented in the +# kernel. The kernel stores nice values as numbers in +# the range 0 (high) to 39 (low), corresponding to the +# user-visible nice range of -20 to 19. +# +# Before Linux 2.6, this was a scaled value based on +# the scheduler weighting given to this process. +# +# (19) nice %ld +# The nice value (see setpriority(2)), a value in the +# range 19 (low priority) to -20 (high priority). +# +# (20) num_threads %ld +# Number of threads in this process (since Linux 2.6). +# Before kernel 2.6, this field was hard coded to 0 as +# a placeholder for an earlier removed field. +# +# (21) itrealvalue %ld +# The time in jiffies before the next SIGALRM is sent +# to the process due to an interval timer. Since +# kernel 2.6.17, this field is no longer maintained, +# and is hard coded as 0. +# +# (22) starttime %llu +# The time the process started after system boot. In +# kernels before Linux 2.6, this value was expressed +# in jiffies. Since Linux 2.6, the value is expressed +# in clock ticks (divide by sysconf(_SC_CLK_TCK)). +# +# The format for this field was %lu before Linux 2.6. +# +# (23) vsize %lu +# Virtual memory size in bytes. +# +# (24) rss %ld +# Resident Set Size: number of pages the process has +# in real memory. This is just the pages which count +# toward text, data, or stack space. This does not +# include pages which have not been demand-loaded in, +# or which are swapped out. +# +# (25) rsslim %lu +# Current soft limit in bytes on the rss of the +# process; see the description of RLIMIT_RSS in +# getrlimit(2). +# +# (26) startcode %lu +# The address above which program text can run. +# +# (27) endcode %lu +# The address below which program text can run. +# +# (28) startstack %lu +# The address of the start (i.e., bottom) of the +# stack. +# +# (29) kstkesp %lu +# The current value of ESP (stack pointer), as found +# in the kernel stack page for the process. +# +# (30) kstkeip %lu +# The current EIP (instruction pointer). +# +# (31) signal %lu +# The bitmap of pending signals, displayed as a +# decimal number. Obsolete, because it does not +# provide information on real-time signals; use +# /proc/[pid]/status instead. +# +# (32) blocked %lu +# The bitmap of blocked signals, displayed as a +# decimal number. Obsolete, because it does not +# provide information on real-time signals; use +# /proc/[pid]/status instead. +# +# (33) sigignore %lu +# The bitmap of ignored signals, displayed as a +# decimal number. Obsolete, because it does not +# provide information on real-time signals; use +# /proc/[pid]/status instead. +# +# (34) sigcatch %lu +# The bitmap of caught signals, displayed as a decimal +# number. Obsolete, because it does not provide +# information on real-time signals; use +# /proc/[pid]/status instead. +# +# (35) wchan %lu +# This is the "channel" in which the process is +# waiting. It is the address of a location in the +# kernel where the process is sleeping. The +# corresponding symbolic name can be found in +# /proc/[pid]/wchan. +# +# (36) nswap %lu +# Number of pages swapped (not maintained). +# +# (37) cnswap %lu +# Cumulative nswap for child processes (not +# maintained). +# +# (38) exit_signal %d (since Linux 2.1.22) +# Signal to be sent to parent when we die. +# +# (39) processor %d (since Linux 2.2.8) +# CPU number last executed on. +# +# (40) rt_priority %u (since Linux 2.5.19) +# Real-time scheduling priority, a number in the range +# 1 to 99 for processes scheduled under a real-time +# policy, or 0, for non-real-time processes (see +# sched_setscheduler(2)). +# +# (41) policy %u (since Linux 2.5.19) +# Scheduling policy (see sched_setscheduler(2)). +# Decode using the SCHED_* constants in linux/sched.h. +# +# The format for this field was %lu before Linux +# 2.6.22. +# +# (42) delayacct_blkio_ticks %llu (since Linux 2.6.18) +# Aggregated block I/O delays, measured in clock ticks +# (centiseconds). +# +# (43) guest_time %lu (since Linux 2.6.24) +# Guest time of the process (time spent running a +# virtual CPU for a guest operating system), measured +# in clock ticks (divide by sysconf(_SC_CLK_TCK)). +# +# (44) cguest_time %ld (since Linux 2.6.24) +# Guest time of the process's children, measured in +# clock ticks (divide by sysconf(_SC_CLK_TCK)). +# +# (45) start_data %lu (since Linux 3.3) +# Address above which program initialized and +# uninitialized (BSS) data are placed. +# +# (46) end_data %lu (since Linux 3.3) +# Address below which program initialized and +# uninitialized (BSS) data are placed. +# +# (47) start_brk %lu (since Linux 3.3) +# Address above which program heap can be expanded +# with brk(2). +# +# (48) arg_start %lu (since Linux 3.5) +# Address above which program command-line arguments +# (argv) are placed. +# +# (49) arg_end %lu (since Linux 3.5) +# Address below program command-line arguments (argv) +# are placed. +# +# (50) env_start %lu (since Linux 3.5) +# Address above which program environment is placed. +# +# (51) env_end %lu (since Linux 3.5) +# Address below which program environment is placed. +# +# (52) exit_code %d (since Linux 3.5) +# The thread's exit status in the form reported by +# waitpid(2). diff --git a/flightcode/python/pixhawk.py b/flightcode/python/pixhawk.py new file mode 100755 index 0000000..a86c4d3 --- /dev/null +++ b/flightcode/python/pixhawk.py @@ -0,0 +1,733 @@ +#!/usr/bin/env python + +# Pixhawk Initialization and Firmware Upgrade + +import ConfigParser +import glob +import json +import logging +import logging.config +import os +import re +import serial +import subprocess +import sys +import time +import traceback +from pymavlink import mavutil +sys.path.append("/usr/bin") +import clock +import led +import usb + +version_file = "/PIX_VERSION" +sololink_conf = "/etc/sololink.conf" +sololink_back = "/etc/sololink.back" +config_baud_name = "telemBaud" +config_baudlist_name = "telemBaudList" +config_dev_name = "telemDev" +config_flow_name = "telemFlow" +firmware_root = "/firmware" + +# ardupilot/libraries/AP_SerialManager/AP_SerialManager.cpp, supported bauds: +# 1500000 921600 500000 460800 230400 115200 111100 100000 57600 38400 19200 +# 9600 4800 2400 1200 +# Reordered in an attempt to try more likely ones sooner (guesses). The one in +# the config file is always tried first. +baudlist_default = "\ +115200 \ +921600 \ +57600 \ +230400 \ +460800 \ +1500000 \ +" + + +# return a matching file, or None +def glob_file(pattern): + files = glob.glob(pattern) + if len(files) == 0: + return None + elif len(files) > 1: + logger.warning("more than one matching filename (%s)", str(files)) + return files[0] + + +# /dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00 +# /dev/serial/by-id/usb-3D_Robotics_PX4_BL_FMU_v2.x_0-if00 +dev_pattern_usb = "/dev/serial/by-id/usb-3D_Robotics*" + +# return device name or None +def create_usb_serial(timeout=2): + usb.init() + usb.enable() + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + now_us = start_us + timeout_us = timeout * 1000000 + while glob_file(dev_pattern_usb) is None and (now_us - start_us) < timeout_us: + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + time.sleep(0.001) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + dev_name = glob_file(dev_pattern_usb) + if os.path.exists(dev_name): + logger.debug("%s created in %0.3f sec", + dev_name, (end_us - start_us) / 1000000.0) + else: + logger.error("creating %s", dev_pattern_usb) + usb.disable() + usb.uninit() + return dev_name + + +# return True (success) or False (did not delete device) +def delete_usb_serial(timeout=1): + usb.disable() + usb.uninit() + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + now_us = start_us + timeout_us = timeout * 1000000 + while glob_file(dev_pattern_usb) is not None and (now_us - start_us) < timeout_us: + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + time.sleep(0.001) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if glob_file(dev_pattern_usb) is None: + logger.debug("%s deleted in %0.3f sec", + dev_pattern_usb, (end_us - start_us) / 1000000.0) + return True + else: + logger.error("deleting device %s", dev_pattern_usb) + return False + + +def config_get(cfg, name): + try: + value = cfg.get("solo", name) + except: + value = None + return value + + +def config_getint(cfg, name): + try: + value = cfg.getint("solo", name) + except: + value = None + return value + + +def config_getbool(cfg, name, default=None): + try: + value = cfg.getboolean("solo", name) + except: + value = default + return value + + +def config_write(config): + os.system("cp %s %s" % (sololink_conf, sololink_back)) + os.system("md5sum %s > %s.md5" % (sololink_back, sololink_back)) + os.system("sync") + f = open(sololink_conf, 'wb') + config.write(f) + f.close() + os.system("md5sum %s > %s.md5" % (sololink_conf, sololink_conf)) + os.system("sync") + os.system("rm %s %s.md5" % (sololink_back, sololink_back)) + + +def create_tty_mavlink(serial_baud=None): + config = ConfigParser.SafeConfigParser() + config.optionxform = str + config.read(sololink_conf) + + serial_dev = config_get(config, config_dev_name) + if serial_dev is None: + logger.error("reading %s from %s", config_dev_name, sololink_conf) + return None + logger.debug("%s = %s", config_dev_name, serial_dev) + + serial_flow = config_getbool(config, config_flow_name, True) + logger.debug("%s = %s", config_flow_name, serial_flow) + + if serial_baud is None: + serial_baud = config_getint(config, config_baud_name) + if serial_baud is None: + logger.error("reading %s from %s", config_baud_name, sololink_conf) + return None + logger.debug("%s = %d", config_baud_name, serial_baud) + + m = mavutil.mavlink_connection(serial_dev, baud=serial_baud) + m.set_rtscts(serial_flow) + return m + + +# Get list of baud rates to try +# +# Returns: +# list of integers +def get_baudlist(config=None, expected_baud=None): + + # use list in config file if it is there + baudlist = config_get(config, config_baudlist_name) + if baudlist is None: + # not in config file; use default + baudlist = baudlist_default + logger.debug("using default baud list") + + # convert from string to list of integers + baudlist = baudlist.split() + baudlist = [int(b) for b in baudlist] + + # put expected baud at start, twice (one retry) + if expected_baud is not None: + # first delete it from wherever it might be + try: + baudlist.remove(expected_baud) + except: + # config file baud is not one we were going to try! + logger.warning("trying unlisted baud %d", expected_baud) + # insert at beginning + baudlist.insert(0, expected_baud) + baudlist.insert(0, expected_baud) + + return baudlist + + +# Find and return Pixhawk's telemetry baud +# +# The assumption is that the baud in the config file is almost always correct. +# If the baud is changed via a GCS, this function scans a list of bauds, +# looking for a heartbeat on each one. When a heartbeat is found, the config +# file is updated and that baud is returned. +# +# Config items: +# serial device (required) +# expected baud (optional, default is to scan list) +# list of bauds to scan (optional, default is 'baudlist_default') +# +# The expected baud is given preference by putting it first in the list of +# bauds to scan. It is actually put at the head of the list twice to give some +# degree of error tolerance. +# +# Returns: +# baud if found +# None if Pixhawk not detected or other error +def get_baud(): + + config = ConfigParser.SafeConfigParser() + config.optionxform = str + config.read(sololink_conf) + + serial_dev = config_get(config, config_dev_name) + if serial_dev is None: + logger.error("reading %s from %s", config_dev_name, sololink_conf) + return None + logger.debug("%s = %s", config_dev_name, serial_dev) + + serial_flow = config_getbool(config, config_flow_name, True) + logger.debug("%s = %s", config_flow_name, serial_flow) + + expected_baud = config_getint(config, config_baud_name) + if expected_baud is None: + logger.warning("no %s in %s (will search)", config_baud_name, sololink_conf) + logger.debug("%s = %s", config_baud_name, str(expected_baud)) + + baudlist = get_baudlist(config, expected_baud) + + logger.debug("baud list %s", str(baudlist)) + + logger.info("checking baud...") + + for baud in baudlist: + + logger.debug("baud %d...", baud) + + m = mavutil.mavlink_connection(serial_dev, baud=baud) + m.set_rtscts(serial_flow) + # Allow for missing one, then getting the next one. The expectation is + # that we'll almost always have the right baud and get the first + # heartbeat. + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=2.5) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + m.close() + + if hb is not None: + # pixhawk is at baud + # update config file if necessary + logger.debug("heartbeat received in %0.3f sec", + (end_us - start_us) / 1000000.0) + logger.info("found at %d", baud) + if baud != expected_baud: + logger.info("updating %s %s from %s to %d", sololink_conf, + config_baud_name, str(expected_baud), baud) + config.set("solo", config_baud_name, str(baud)) + config_write(config) + return baud + logger.info("not at %d", baud) + logger.error("not detected at any baud") + return None + + +# Set baud rate in config file if necessary +def set_baud(new_baud): + + config = ConfigParser.SafeConfigParser() + config.optionxform = str + config.read(sololink_conf) + + old_baud = config_getint(config, config_baud_name) + if old_baud is None: + logger.warning("no %s in %s", config_baud_name, sololink_conf) + return + logger.debug("%s = %s", config_baud_name, str(old_baud)) + + if new_baud != old_baud: + logger.info("updating %s %s from %s to %d", sololink_conf, + config_baud_name, str(old_baud), new_baud) + config.set("solo", config_baud_name, str(new_baud)) + config_write(config) + + +# Get version of running firmware +# +# The autopilot_version_request message is used to get the git hashes. A +# param_request_list message is used to get the build number (x.y.z); it +# happens to be returned in one of the statustext message sent as part of the +# reply. +# +# Returned version is a dictionary: +# keys are: build_version, ardupilot_git_hash, px4_git_hash, nuttx_git_hash +# each value is a string, or missing if could not get version from pixhawk +def get_version(): + + config = ConfigParser.SafeConfigParser() + config.optionxform = str + config.read(sololink_conf) + + serial_dev = config_get(config, config_dev_name) + if serial_dev is None: + logger.error("reading %s from %s", config_dev_name, sololink_conf) + return None + logger.debug("%s = %s", config_dev_name, serial_dev) + + serial_flow = config_getbool(config, config_flow_name, True) + logger.debug("%s = %s", config_flow_name, serial_flow) + + serial_baud = config_getint(config, config_baud_name) + if serial_baud is None: + logger.error("reading %s from %s", config_baud_name, sololink_conf) + return None + logger.debug("%s = %d", config_baud_name, serial_baud) + + m = mavutil.mavlink_connection(serial_dev, baud=serial_baud) + m.set_rtscts(serial_flow) + + version = {} + + # Read version. Use AUTOPILOT_VERSION message to get the hashes, and use + # the STATUSTEXT returned at the start a parameter dump to get the x.y.z + # version. + + m.mav.autopilot_version_request_send(m.target_system, m.target_component) + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + av = m.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=2) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if av is not None: + version["ardupilot_git_hash"] = "".join(chr(e) for e in av.flight_custom_version) + version["px4_git_hash"] = "".join(chr(e) for e in av.middleware_custom_version) + version["nuttx_git_hash"] = "".join(chr(e) for e in av.os_custom_version) + logger.debug("git hashes received in %0.3f sec", + (end_us - start_us) / 1000000.0) + else: + logger.warning("no version hashes received") + + m.mav.param_request_list_send(m.target_system, m.target_component) + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + end_us = start_us + timeout_us = 2 * 1000000 + # Loop because we might get a STATUSTEXT without the version first. + while (end_us - start_us) < timeout_us: + st = m.recv_match(type='STATUSTEXT', blocking=True, timeout=2) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if st is not None: + # "APM:Copter solo-0.1.2 (b2dacc52)" + match = re.match("APM:.*?solo-([0-9]+\.[0-9]+\.[0-9]+)", st.text) + if match: + logger.debug("build version received in %0.3f sec", + (end_us - start_us) / 1000000.0) + version["build_version"] = match.group(1) + logger.info("build version %s", version_string(version)) + m.close() + return version + # "ArduCopter V3.2.1 (b2dacc52)" + # This is matched in case someone is messing with their firmware + # Anything looking like a version x.y.z is pulled out + match = re.match(".*?([0-9]+\.[0-9]+\.[0-9]+)", st.text) + if match: + logger.warning("firmware is not specifically for solo") + logger.debug("build version received in %0.3f sec", + (end_us - start_us) / 1000000.0) + version["build_version"] = match.group(1) + logger.info("build version %s", version_string(version)) + m.close() + return version + ### while end_us... + + m.close() + logger.warning("no build version received") + return version + + +# load firmware +def load(firmware_path): + logger.info("loading %s", firmware_path) + + dev_name = create_usb_serial() + if dev_name is None: + return False + + # load firmware + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + ret = subprocess.call(["px_uploader.py", + "--port=%s" % dev_pattern_usb, + firmware_path]) + if ret != 0: + loaded = False + logger.error("loading firmware") + else: + loaded = True + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + logger.info("firmware loaded in %0.3f sec", + (end_us - start_us) / 1000000.0) + + if loaded: + # firmware loaded; wait for heartbeat on telemetry port + # This allows for the baud rate to change along with the firmware load. + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + # 0.6.x ... 1.0.5 has a pixhawk build at 115200 baud + # internal versions might have a pixhawk build at 921600 baud + # 1.1.5 and later have a pixhawk build at 115200 baud + baudlist = get_baudlist(None, 115200) + hb_quit = False + hb_time_us = None + wait_us = None + hb = None + while not hb_quit: + for baud in baudlist: + logger.debug("trying %d", baud) + m = create_tty_mavlink(baud) + if m is None: + logger.error("creating tty mavlink connection") + hb_quit = True + break # for baud + # flush input - could we have data from before flashing? + flush_bytes = m.port.inWaiting() + if flush_bytes > 0: + logger.info("flushing %d bytes", flush_bytes) + m.port.flushInput() + logger.debug("waiting for heartbeat") + hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=1.1) + wait_us = clock.gettime_us(clock.CLOCK_MONOTONIC) - start_us + if hb is None: + logger.debug("timeout waiting for first heartbeat") + else: + # insisting on a second heartbeat was in response to + # occasionally detecting a heartbeat at the wrong baud + # when the baud changes with the firmware (!) + logger.debug("got first heartbeat") + hb_time_us = clock.gettime_us(clock.CLOCK_MONOTONIC) - start_us + hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=1.1) + if hb is None: + # this has been observed to happen (rarely) + logger.info("timeout waiting for second heartbeat at %d after loading", baud) + # ...and continue with the next baud rate + else: + logger.debug("got second heartbeat") + m.close() + if hb is not None: + set_baud(baud) + hb_quit = True + break # for baud + # 0.1.0 comes up in about 25 sec + # 1.0.8 comes up in about 8 sec + # 1.1.4 comes up in about 15 sec + if wait_us > 45000000: # 45 sec + hb_quit = True # to exit the while loop + break # exit the baud loop + ### for baud in baudlist + ### while !hb_quit + if hb is None: + logger.error("timeout waiting for heartbeat after loading") + else: + logger.info("heartbeat after loading in %0.3f sec", hb_time_us / 1000000.0) + + delete_usb_serial() + + return loaded + + +# check usb (diagnostic) +def check_usb(): + dev_name = create_usb_serial() + if dev_name is None: + print "ERROR creating usb serial device" + return + print "created %s" % dev_name + time.sleep(1) + s = serial.Serial(port=dev_name, timeout=1) + print "opened %s" % dev_name + d = s.read(100) + print [hex(ord(b)) for b in d] + s.close() + print "closed %s" % dev_name + time.sleep(1) + delete_usb_serial() + print "deleted %s" % dev_name + + +# verify usb (test) +# return True (pass) or False (fail) +def verify_usb(): + dev_name = create_usb_serial() + if dev_name is None: + logger.info("ERROR creating usb serial device") + return False + # need to experimentally determine how long we have to wait here before + # trying to use the USB device (pretty sure it's nonzero) + time.sleep(1) + m = mavutil.mavlink_connection(dev_name) + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=2.5) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + m.close() + delete_usb_serial() + if hb is not None: + logger.info("heartbeat received over USB in %0.3f sec", + (end_us - start_us) / 1000000.0) + return True + else: + logger.error("ERROR getting heartbeat over USB") + return False + + +# return info about firmware file +# (full_path, versions) +# full_path is the full path to the file, or None if no file +# if full_path is not None, versions is a dictionary of version info +def find_firmware(): + files = glob.glob("%s/*.px4" % firmware_root) + if len(files) == 0: + return (None, None) + # read git hashes + full_path = files[-1] + f = open(full_path) + info = json.loads(f.read()) + f.close() + versions = {} + for i in [ "ardupilot_git_hash", "px4_git_hash", "nuttx_git_hash" ]: + try: + versions[i] = str(info[i][:8]) + except: + versions[i] = "(missing)" + return (full_path, versions) + + +def mkdir_p(path): + try: + os.makedirs(path) + except OSError: + pass # already exists + + +def move_firmware(firmware_path): + mkdir_p("%s/loaded" % firmware_root) + firmware_file = os.path.basename(firmware_path) + os.rename(firmware_path, "%s/loaded/%s" % (firmware_root, firmware_file)) + + +def write_version_file(version): + f = open(version_file, "w") + for v in ["build_version", "ardupilot_git_hash", "px4_git_hash", "nuttx_git_hash"]: + if v in version: + f.write(version[v] + '\n') + else: + f.write("unknown\n") + f.close() + + +def version_string(version): + vs = "" + if "build_version" in version: + vs += "build=%s " % version["build_version"] + vs += "ardupilot=" + if "ardupilot_git_hash" in version: + vs += version["ardupilot_git_hash"] + else: + vs += "unknown" + vs += " px4=" + if "px4_git_hash" in version: + vs += version["px4_git_hash"] + else: + vs += "unknown" + vs += " nuttx=" + if "nuttx_git_hash" in version: + vs += version["nuttx_git_hash"] + else: + vs += "unknown" + return vs + + +def initialize(): + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + led.blink(1000, 100) + print "pixhawk..." + baud = get_baud() + if baud == None: + print "pixhawk: ERROR checking baud" + logger.error("finding baud") + logger.error("pixhawk status: no response") + # pixhawk might be stuck in bootloader + + # try to load if we have firmware, whether we found the baud or not + + (firmware_path, firmware_version) = find_firmware() + if firmware_path is None: + logger.info("no new firmware file") + # Another backup would be to look in /firmware/loaded and load + # whatever's there if we did not get a heartbeat. Getting stuck in + # the bootloader has so far only been observed to happen when a + # programming is interrupted, in which case the firmware we were + # loading is still in /firmware. + elif os.path.exists("/log/.factory") and \ + (baud is not None) and \ + verify_usb(): + logger.info("pixhawk: factory - not loading firmware") + move_firmware(firmware_path) + else: + print "pixhawk: loading firmware" + logger.info("%s:", firmware_path) + for v in firmware_version: + logger.info("%-20s %s", v, firmware_version[v]) + if load(firmware_path): + move_firmware(firmware_path) + else: + print "pixhawk: ERROR loading firmware" + logger.error("pixhawk status: can't load") + + # If .factory exists, we either found a baud, passed USB, and skipped + # loading pixhawk, or either did not find a baud or USB heartbeat, and + # loaded pixhawk. + os.system("rm -f /log/.factory") + + # We have followed some combination of these to get here: + # * found baud | did not find baud + # * no firmware | firmware and flashed it | firmware and error flashing + # + # The cases that are known to happen: + # Normal cases + # * Found baud, there was no new firmware + # * Found baud, there was new firmware, flashed it + # Error cases + # * Did not find baud, there was new firmware, flashed it + # + # Other cases should "never" happen (and have not been observed): + # * Did not find baud, there was no new firmware + # The only known way to not find the baud is if pixhawk is stuck in + # its bootloader, which happens because flashing was interrupted, + # which means there is new firmware available. + # * Found baud, there was new firmware, error flashing it + # * Did not find baud, there was new firmware, error flashing it + # Should "never" fail to flash pixhawk when we try to. + + # This should work for any of the three known-to-happen cases. For the + # error cases, running_version will be set to an empty dictionary, and + # write_version_file will write "unknown" for all the versions. + # get_version() can return None if the configuration is corrupt, but in + # that case we have far deeper problems (an md5 has just succeeded). + running_version = get_version() + + logger.info("now running:") + for component in ["build_version", "ardupilot_git_hash", "px4_git_hash", "nuttx_git_hash"]: + if component in running_version: + version = running_version[component] + else: + version = "unknown" + logger.info("%-20s %s", component, version) + write_version_file(running_version) + if "build_version" in running_version \ + and running_version["build_version"] != "unknown": + logger.info("pixhawk status: ready") + print "pixhawk: running %s" % running_version["build_version"] + else: + print "pixhawk: ERROR checking version" + led.off() + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + logger.debug("pixhawk initialization took %0.3f sec", + (end_us - start_us) / 1000000.0) + + +if __name__ == "__main__": + + from optparse import OptionParser + + parser = OptionParser("pixhawk.py [options]") + parser.add_option("-i", "--init", dest="init", action="store_true", + default=False, help="initialize pixhawk") + parser.add_option("-v", "--version", dest="version", action="store_true", + default=False, help="find pixhawk version") + parser.add_option("-b", "--baud", dest="baud", action="store_true", + default=False, help="find pixhawk baud") + parser.add_option("-l", "--load", dest="load", type="string", + default=None, help="load pixhawk firmware") + parser.add_option("-u", "--usb", dest="usb", action="store_true", + default=False, help="connect usb and see what's there") + parser.add_option("-c", "--config", dest="conf", type="string", default=None, + help="configuration file (%s)" % sololink_conf) + parser.add_option("-f", "--firmware", dest="firm", type="string", default=None, + help="firmware root directory (%s)" % firmware_root) + parser.add_option("-p", "--pix_version", dest="pix", type="string", default=None, + help="pixhawk version file (%s)" % version_file) + (opts, args) = parser.parse_args() + + if opts.conf is not None: + sololink_conf = opts.conf + + if opts.firm is not None: + firmware_root = opts.firm + + if opts.pix is not None: + version_file = opts.pix + + logging.config.fileConfig(sololink_conf) + logger = logging.getLogger("pix") + + logger.info("pixhawk.py starting") + + if opts.init: + # initialize, logging any unexpected exceptions + try: + initialize() + except Exception as ex: + logger.error("unhandled exception!") + except_str = traceback.format_exc() + print except_str + except_str_list = except_str.split('\n') + for str in except_str_list: + logger.error(str) + # try end + + if opts.version: + print get_version() + + if opts.baud: + print get_baud() + + if opts.load is not None: + load(opts.load) + + if opts.usb: + print check_usb() + + logger.info("pixhawk.py finished") diff --git a/flightcode/python/rc_lock.py b/flightcode/python/rc_lock.py new file mode 100644 index 0000000..7fb1e4d --- /dev/null +++ b/flightcode/python/rc_lock.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +import errno +import os + + +# must match RcLock.cpp +ro_lockfile = "/mnt/rootfs.ro/etc/.rc_lock" +tmp_lockfile = "/tmp/.rc_lock" +tmp_unlockfile = "/tmp/.rc_unlock" + + +def locked(): + if os.path.isfile(tmp_unlockfile): + return False + elif os.path.isfile(tmp_lockfile) or \ + os.path.isfile(ro_lockfile): + return True + else: + return False + + +def lock_version(): + with open(tmp_lockfile, 'a'): # "touch" + os.utime(tmp_lockfile, None) + + +def unlock_version(): + try: + os.unlink(tmp_lockfile) + except OSError as e: + if e.errno != errno.ENOENT: + raise e + + +def unlock_override(): + with open(tmp_unlockfile, 'a'): # "touch" + os.utime(tmp_unlockfile, None) diff --git a/flightcode/python/runStickCal.sh b/flightcode/python/runStickCal.sh new file mode 100755 index 0000000..abb2763 --- /dev/null +++ b/flightcode/python/runStickCal.sh @@ -0,0 +1,18 @@ +#!/bin/sh + +#Comment out the RCTX line in /etc/inittab +sed -i 's/^RCTX.*/#&/g' /etc/inittab + +#stop the stm32 process +init q +sleep 1 + +#update artoo +echo "Press Ctrl+C to write the calibration" +stick-cal.py /dev/ttymxc1 + +#Uncomment the RCTX line +sed -i 's/.\(RCTX*.\)/\1/g' /etc/inittab + +#start the stm32 process back up +init q diff --git a/flightcode/python/slip.py b/flightcode/python/slip.py new file mode 100755 index 0000000..5ed5cd6 --- /dev/null +++ b/flightcode/python/slip.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python + +class SlipDevice: + + END = chr(0300) # indicates end of packet + ESC = chr(0333) # indicates byte stuffing + ESC_END = chr(0334) # ESC ESC_END means END data byte + ESC_ESC = chr(0335) # ESC ESC_ESC means ESC data byte + + def __init__(self, serial_dev): + self.ser = serial_dev + self.insync = False + + def sync(self): + attempts = 0 + while 1: + dat = self.ser.read() + if not dat: + attempts += 1 + if(attempts >= 3): + print "slip sync read fail, breaking" + return False + continue + + if dat == self.END: + self.insync = True + return True + + def read(self): + """ + read a SLIP packet from artoo. + """ + pkt = [] + + if not self.insync: + if(self.sync() == False): + return pkt; + + while True: + b = self.ser.read() + if b == self.END: + if len(pkt) > 0: + return pkt + elif b == self.ESC: + b = self.ser.read() + if b == self.ESC_END: + pkt.append(self.END) + elif b == self.ESC_ESC: + pkt.append(self.ESC) + else: + pkt.append(b) + else: + pkt.append(b) + + def write(self, pkt): + """ + write a SLIP message to artoo + """ + + slip_bytes = [self.END] + for b in pkt: + if b == self.END: + slip_bytes.append(self.ESC) + slip_bytes.append(self.ESC_END) + elif b == self.ESC: + slip_bytes.append(self.ESC) + slip_bytes.append(self.ESC_ESC) + else: + slip_bytes.append(b) + + slip_bytes.append(self.END) + self.ser.write("".join(slip_bytes)) diff --git a/flightcode/python/stick-cal.py b/flightcode/python/stick-cal.py new file mode 100755 index 0000000..86b3ff7 --- /dev/null +++ b/flightcode/python/stick-cal.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python + +import sys, os, struct +import serial, slip + +ARTOO_BAUD = 115200 + +ARTOO_RAW_IO_ID = chr(0x5) +ARTOO_RAW_IO_REPORT_ID = chr(0x6) +ARTOO_SYSINFO_ID = chr(0x3) +ARTOO_CALIBRATE_ID = chr(0x2) +ARTOO_DSM_ID = chr(0x1) + +class Axis(): + def __init__(self): + self.min = sys.maxint + self.trim = 0 + self.max = 0 + self.current = 0 + + def update(self, val): + self.min = min(self.min, val) + self.max = max(self.max, val) + self.current = val + + def extremes(self): + return "%04d:%04d" % (self.min, self.max) + + def vals(self): + return [self.min, self.trim, self.max] + + def latch_trim(self): + self.trim = self.current + +def get_and_print_sysinfo(slipdev): + while True: + slipdev.write("".join([ARTOO_SYSINFO_ID])) + pkt = slipdev.read() + pkt = "".join(pkt) + if pkt[0] == ARTOO_SYSINFO_ID: + uid = "".join(["{:02x}".format(ord(c)) for c in pkt[:12]]) + hw_version = struct.unpack_from(' +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../mavlink/c_library/common/mavlink.h" +#include "util.h" + +/* +* Retrieve wifi information and: +* - log it +* - send mavlinke rssi message +* +* Netlink documentation: http://www.infradead.org/~tgr/libnl/ +* Netlink example: iw +*/ + +/* + * Implementation notes + * + * Each time this runs it creates a unix socket in /tmp. Since there is no + * "clean" exit from this program, that socket is not deleted until the next + * reboot, and then only because /tmp is not persistent. There is a very + * unlikely failure mode where when this program dies and restarts, it comes + * back with the same PID; if that happens, it will probably fail to bind to + * the unix socket (since it /tmp/rssi_send. will already exist) and + * exit. Whoever started this program (inittab) will notice and start it + * again. [That case has never been observed to happen.] + * + * The netlink message creation, sending, and parsing is rather mysterious; + * there is little in the way of documentation other than the source code. + * Some basic unknowns are things like when we allocate a message and give it + * to the send function, who should free it? It would be possible to drill + * down further into the source and figure it out, but instead, testing is + * used; a run at 1000 msgs/sec for tens of minutes shows that the program's + * size is not growning, so there are probably no leaks. + * + * Late conversion from C to C++ to add logging. + * + * The netlink stuff returns information in attribute tables. When we get + * one of those, we copy the attributes that are present into a simple + * structure defined here (station_info and rate_info). Each of those + * structures has a name that corresponds with one of the attribute enums + * in linux/nl80211.h, and the struct has fields corresponding to the + * enum values. + */ + +/* system and component IDs to use in mavlink header */ +static uint8_t system_id = 10; /* change with -s option */ +static uint8_t component_id = 0; /* change with -c option */ + +/* time between rssi messages (default 1 second) */ +static unsigned interval_ms = 1000; /* change with -i option */ + +/* run verbose (debug) */ +static int verbose = 0; + +#define DBG(X) + +/* + * Rate info, contained in station info + * + * Fields commented out are ones where linux/nl80211.h does not say what the + * type is, and we don't need right now, so they are just left out. + */ +typedef struct rate_info { + uint16_t bitrate; /* total bitrate (u16, 100kbit/s) */ + uint8_t mcs; /* mcs index for 802.11n (u8) */ + /* n40_mhz_width: 40 MHz dualchannel bitrate */ + /* short_gi: 400ns guard interval */ + uint32_t bitrate32; /* total bitrate (u32, 100kbit/s) */ + /* max: highest rate_info number currently defined */ + uint8_t vht_mcs; /* MCS index for VHT (u8) */ + uint8_t vht_nss; /* number of streams in VHT (u8) */ + /* n80_mhz_width: 80 MHz VHT rate */ + /* n80p80_mhz_width: 80+80 MHz VHT rate */ + /* n160_mhz_width: 160 MHz VHT rate */ +} rate_info_t; + +/* print structure to a string, debug */ +static char *rate_info_str(char *s, const rate_info_t *p) +{ + /* 43 chars + 5 u32 = 43 + 5 * 10 = 93 chars max */ + sprintf(s, "bitrate=%u mcs=%u bitrate32=%u vht_mcs=%u vht_nss=%u", p->bitrate, p->mcs, + p->bitrate32, p->vht_mcs, p->vht_nss); + return s; +} + +/* + * Information retrieved via station info request message + * + * Fields commented out are ones where linux/nl80211.h does not say what the + * type is, and we don't need right now, so they are just left out. + */ +typedef struct station_info { + uint32_t inactive_time; /* time since last activity (u32, msecs) */ + uint32_t rx_bytes; /* total received bytes (u32, from this station) */ + uint32_t tx_bytes; /* total transmitted bytes (u32, to this station) */ + uint64_t rx_bytes64; /* total received bytes (u64, from this station) */ + uint64_t tx_bytes64; /* total transmitted bytes (u64, to this station) */ + int8_t signal; /* signal strength of last received PPDU (u8, dBm) */ + rate_info_t tx_bitrate; /* current unicast tx rate, nested attribute containing + info as possible, see &enum nl80211_rate_info */ + uint32_t rx_packets; /* total received packet (u32, from this station) */ + uint32_t tx_packets; /* total transmitted packets (u32, to this station) */ + uint32_t tx_retries; /* total retries (u32, to this station) */ + uint32_t tx_failed; /* total failed packets (u32, to this station) */ + int8_t signal_avg; /* signal strength average (u8, dBm) */ + /* llid: the station's mesh LLID */ + /* plid: the station's mesh PLID */ + /* plink_state: peer link state for the station (see %enum nl80211_plink_state) */ + rate_info_t rx_bitrate; /* last unicast data frame rx rate, nested attribute, + like NL80211_STA_INFO_TX_BITRATE. */ + /* bss_param: current station's view of BSS, nested attribute containing info as + possible, see &enum nl80211_sta_bss_param */ + /* connected_time: time since the station is last connected */ + /* sta_flags: Contains a struct nl80211_sta_flag_update. */ + uint32_t beacon_loss; /* count of times beacon loss was detected (u32) */ + int64_t t_offset; /* timing offset with respect to this STA (s64) */ + /* local_pm: local mesh STA link-specific power mode */ + /* peer_pm: peer mesh STA link-specific power mode */ + /* nonpeer_pm: neighbor mesh STA power save mode towards non-peer STA */ +} station_info_t; + +/* + * Information retrieved via station info request message + * + * Fields commented out are ones where linux/nl80211.h does not say what the + * type is, and we don't need right now, so they are just left out. + */ +typedef struct survey_info { + /* + * NL80211_SURVEY_INFO_FREQUENCY: center frequency of channel + * NL80211_SURVEY_INFO_NOISE: noise level of channel (u8, dBm) + * NL80211_SURVEY_INFO_IN_USE: channel is currently being used + * NL80211_SURVEY_INFO_TIME: amount of time (in ms) that the radio + * was turned on (on channel or globally) + * NL80211_SURVEY_INFO_TIME_BUSY: amount of the time the primary + * channel was sensed busy (either due to activity or energy detect) + * NL80211_SURVEY_INFO_TIME_EXT_BUSY: amount of time the extension + * channel was sensed busy + * NL80211_SURVEY_INFO_TIME_RX: amount of time the radio spent + * receiving data (on channel or globally) + * NL80211_SURVEY_INFO_TIME_TX: amount of time the radio spent + * transmitting data (on channel or globally) + * NL80211_SURVEY_INFO_TIME_SCAN: time the radio spent for scan (on this channel or globally) + */ + + bool in_use; + uint32_t freq; + int8_t noise; + uint64_t chan_active_time; + uint64_t chan_busy_time; + uint64_t chan_ext_busy_time; + uint64_t chan_receive_time; + uint64_t chan_transmit_time; + +} survey_info_t; + +// state related to our netlink context +typedef struct { + struct nl_sock *sock; + int nl80211_id; + unsigned int if_index; + + struct nl_cb *cmd_cb; + struct nl_cb *sock_cb; +} nl_state_t; + +// items that are referenced in netlink callback context +typedef struct { + volatile int + status; // ensure this is always dereferenced, even when only being written in callbacks + station_info_t station_info; + survey_info_t survey_info; +} cb_ctx_t; + +/* print structure to console, debug */ +static void station_info_dump(const station_info_t *s) +{ + char buf[100]; + printf("inactive_time=%u\n", s->inactive_time); + printf("rx_bytes=%u tx_bytes=%u rx_bytes64=%llu tx_bytes64=%llu\n", s->rx_bytes, s->tx_bytes, + s->rx_bytes64, s->tx_bytes64); + printf("signal=%d signal_avg=%d beacon_loss=%u t_offset=%lld\n", s->signal, s->signal_avg, + s->beacon_loss, s->t_offset); + printf("rx_packets=%u tx_packets=%u tx_retries=%u tx_failed=%u\n", s->rx_packets, s->tx_packets, + s->tx_retries, s->tx_failed); + printf("tx_bitrate: %s\n", rate_info_str(buf, &s->tx_bitrate)); + printf("rx_bitrate: %s\n", rate_info_str(buf, &s->rx_bitrate)); +} + +static void send_rssi(int rssi, unsigned retries); +static void get_station_info(cb_ctx_t *cb_ctx, const nl_state_t *nl); +static void get_survey_info(cb_ctx_t *cb_ctx, const nl_state_t *nl); +static int valid_handler(struct nl_msg *msg, void *arg); +static int station_handler(struct nl_msg *msg, station_info_t *station_info); +static int survey_handler(struct nl_msg *msg, survey_info_t *survey_info); +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg); +static int finish_handler(struct nl_msg *msg, void *arg); +static int ack_handler(struct nl_msg *msg, void *arg); + +static bool nl_state_init(nl_state_t *nl); +static bool nl_state_init_callbacks(nl_state_t *nl, cb_ctx_t *cb); + +static void usage(void) +{ + printf("rssi_send [-i interval_msec] [-s system_id] [-c component_id] [-v]\n"); + exit(1); +} + +static uint64_t diff_clamp(uint64_t curr, uint64_t prev) +{ + if (curr < prev) { + return 0; + } + return curr - prev; +} + +static void log_station_info(const station_info_t *curr, const station_info_t *prev) +{ + /* station_info.[tr]x_bitrate.bitrate32 is in units of 100,000 */ + + syslog(LOG_INFO, "station sig=%d txr=%d.%d rxr=%d.%d ret=%llu err=%d", curr->signal_avg, + curr->tx_bitrate.bitrate32 / 10, curr->tx_bitrate.bitrate32 % 10, + curr->rx_bitrate.bitrate32 / 10, curr->rx_bitrate.bitrate32 % 10, + diff_clamp(curr->tx_retries, prev->tx_retries), curr->tx_failed); +} + +static void log_survey_info(const survey_info_t *curr, const survey_info_t *prev) +{ + syslog(LOG_INFO, "survey freq=%d noise=%d active=%llu busy=%llu ext_busy=%llu rx=%llu tx=%llu", + curr->freq, curr->noise, diff_clamp(curr->chan_active_time, prev->chan_active_time), + diff_clamp(curr->chan_busy_time, prev->chan_busy_time), + diff_clamp(curr->chan_ext_busy_time, prev->chan_ext_busy_time), + diff_clamp(curr->chan_receive_time, prev->chan_receive_time), + diff_clamp(curr->chan_transmit_time, prev->chan_transmit_time)); +} + +/* + * main program - + * initialize, then loop: read wifi status, log it, send rssi message + */ +int main(int argc, char *argv[]) +{ + int opt; + uint64_t now_us; + uint64_t next_us; + unsigned interval_us; + + cb_ctx_t cb_ctx; + nl_state_t nl_state; + + station_info_t last_station_info; + survey_info_t last_survey_info; + + /* all arguments are optional; normal use is with no arguments */ + while ((opt = getopt(argc, argv, "i:s:c:v:")) != -1) { + switch (opt) { + case 'i': + interval_ms = atoi(optarg); + break; + case 's': + system_id = atoi(optarg); + break; + case 'c': + component_id = atoi(optarg); + break; + case 'v': + verbose = atoi(optarg); + break; + default: + usage(); + } + } + + memset(&cb_ctx, 0, sizeof cb_ctx); + memset(&last_station_info, 0, sizeof last_station_info); + memset(&last_survey_info, 0, sizeof last_survey_info); + + openlog("rssi_send", LOG_NDELAY, LOG_LOCAL3); + + syslog(LOG_INFO, "main: built " __DATE__ " " __TIME__); + + if (!nl_state_init(&nl_state)) { + syslog(LOG_ERR, "failed to init netlink\n"); + exit(1); + } + + if (!nl_state_init_callbacks(&nl_state, &cb_ctx)) { + syslog(LOG_ERR, "failed to init callbacks\n"); + exit(1); + } + + interval_us = interval_ms * 1000; + + next_us = clock_gettime_us(CLOCK_MONOTONIC) + interval_us; + + while (1) { + // delay until the next message time + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (next_us > now_us) + usleep(next_us - now_us); + next_us += interval_us; + + // both these are blocking + get_station_info(&cb_ctx, &nl_state); + get_survey_info(&cb_ctx, &nl_state); + + if (verbose >= 1) + station_info_dump(&cb_ctx.station_info); + + log_station_info(&cb_ctx.station_info, &last_station_info); + log_survey_info(&cb_ctx.survey_info, &last_survey_info); + + unsigned retries = diff_clamp(cb_ctx.station_info.tx_retries, last_station_info.tx_retries); + + memcpy(&last_station_info, &cb_ctx.station_info, sizeof last_station_info); + memcpy(&last_survey_info, &cb_ctx.survey_info, sizeof last_survey_info); + + send_rssi(cb_ctx.station_info.signal_avg, retries); + } + + return 0; + +} /* main */ + +bool nl_state_init(nl_state_t *nl) +{ + /* + * Initialize our netlink context. + */ + + memset(nl, 0, sizeof *nl); + + nl->sock = nl_socket_alloc(); + if (nl->sock == NULL) { + fprintf(stderr, "ERROR allocating netlink socket\n"); + goto cleanup; + } + DBG(printf("nl_socket_alloc: nl_sock=%p\n", nl_sock);) + + if (genl_connect(nl->sock) != 0) { + fprintf(stderr, "ERROR connecting netlink socket\n"); + goto cleanup; + } + DBG(printf("genl_connect: ok\n");) + + nl->nl80211_id = genl_ctrl_resolve(nl->sock, "nl80211"); + if (nl->nl80211_id < 0) { + fprintf(stderr, "ERROR resolving netlink socket\n"); + goto cleanup; + } + DBG(printf("genl_ctrl_resolve: nl80211_id=%d\n", nl80211_id);) + + nl->if_index = if_nametoindex("wlan0"); + if (nl->if_index == 0) { + fprintf(stderr, "ERROR getting interface index\n"); + goto cleanup; + } + DBG(printf("if_nametoindex: if_index=%u\n", if_index);) + + return true; + +cleanup: + if (nl->sock != NULL) + nl_socket_free(nl->sock); + + return false; +} + +bool nl_state_init_callbacks(nl_state_t *nl, cb_ctx_t *cb) +{ + /* + * Register our callbacks to handle netlink responses/events. + */ + + nl->cmd_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (nl->cmd_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink command callback\n"); + goto cleanup; + } + DBG(printf("nl_cb_alloc: cmd_cb=%p\n", cmd_cb);) + + nl->sock_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (nl->sock_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink socket callback\n"); + goto cleanup; + } + DBG(printf("nl_cb_alloc: sock_cb=%p\n", nl->sock_cb);) + + // handle all NL_CB_VALID callbacks via valid_handler() + if (nl_cb_set(nl->cmd_cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cb) != 0) { + fprintf(stderr, "ERROR setting command callback\n"); + goto cleanup; + } + DBG(printf("nl_cb_set: ok\n");) + + // not clear that this is necessary, but following example convention for now... + nl_socket_set_cb(nl->sock, nl->sock_cb); + + nl_cb_err(nl->cmd_cb, NL_CB_CUSTOM, error_handler, cb); + nl_cb_set(nl->cmd_cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, cb); + nl_cb_set(nl->cmd_cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, cb); + + return true; + +cleanup: + if (nl->sock_cb != NULL) { + nl_cb_put(nl->sock_cb); + } + + if (nl->cmd_cb != NULL) { + nl_cb_put(nl->cmd_cb); + } + return false; +} + +/* send mavlink radio_status (rssi) message */ +static void send_rssi(int rssi, unsigned retries) +{ + /* socket used to send to telem downlink */ + static int fd = -1; + /* destination address */ + static struct sockaddr_un sa_dst; + + char sock_name[32]; + struct sockaddr_un sa_src; + static unsigned const msg_max = 256; + unsigned msg_len; + uint8_t msg_buf[msg_max]; + mavlink_message_t msg; + int num_bytes; + uint8_t u8_rssi; + uint8_t u8_retries; + + /* one-time init */ + if (fd == -1) { + fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (fd == -1) { + perror("socket"); + return; + } + + memset(sock_name, 0, sizeof(sock_name)); + snprintf(sock_name, sizeof(sock_name) - 1, "/tmp/rssi_send.%u", getpid()); + memset(&sa_src, 0, sizeof(sa_src)); + sa_src.sun_family = AF_UNIX; + strncpy(sa_src.sun_path, sock_name, sizeof(sa_src.sun_path) - 1); + if (bind(fd, (struct sockaddr *)&sa_src, sizeof(sa_src)) != 0) { + perror("bind"); + close(fd); + fd = -1; + return; + } + + /* set destination address that will be used for sendto */ + memset(sock_name, 0, sizeof(sock_name)); + strcpy(sock_name, "/run/telem_downlink"); + memset(&sa_dst, 0, sizeof(sa_dst)); + sa_dst.sun_family = AF_UNIX; + strncpy(sa_dst.sun_path, sock_name, sizeof(sa_dst.sun_path) - 1); + + } /* if (fd...) */ + + u8_rssi = (uint8_t)rssi; + u8_retries = (uint8_t)retries; + + mavlink_msg_radio_status_pack(system_id, component_id, &msg, 0, /* rssi */ + u8_rssi, /* remrssi */ + 0, /* txbuf */ + 0, /* noise */ + 0, /* remnoise */ + u8_retries, /* rxerrors */ + 0); /* fixed */ + + msg_len = mavlink_msg_to_send_buffer(msg_buf, &msg); + + if (verbose >= 2) + printf("rssi_send: %02x %02x %02x %02x %02x %02x\n", msg_buf[0], msg_buf[1], msg_buf[2], + msg_buf[3], msg_buf[4], msg_buf[5]); + + num_bytes = sendto(fd, msg_buf, msg_len, 0, (struct sockaddr *)&sa_dst, sizeof(sa_dst)); + if (num_bytes < 0) + perror("sendto"); + +} /* send_rssi */ + +static bool send_nlcmd(const nl_state_t *nl, nl80211_commands c, int nl_msg_flags) +{ + /* + * Assemble and send the netlink command 'c'. + */ + + void *hdr = NULL; + int num_bytes = -1; + uint32_t if_index_uint32; + struct nl_msg *msg; + + bool success = true; + + msg = nlmsg_alloc(); + if (msg == NULL) { + fprintf(stderr, "ERROR allocating netlink message\n"); + success = false; + goto cleanup; + } + DBG(printf("nlmsg_alloc: msg=%p\n", msg);) + + hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, nl->nl80211_id, 0, nl_msg_flags, c, 0); + if (hdr == NULL) { + fprintf(stderr, "ERROR creating netlink message\n"); + success = false; + goto cleanup; + } + DBG(printf("genlmsg_put: hdr=%p\n", hdr);) + + if_index_uint32 = nl->if_index; + if (nla_put(msg, NL80211_ATTR_IFINDEX, sizeof(uint32_t), &if_index_uint32) != 0) { + fprintf(stderr, "ERROR setting message attribute\n"); + success = false; + goto cleanup; + } + DBG(printf("nla_put: ok\n");) + + num_bytes = nl_send_auto(nl->sock, msg); + if (num_bytes < 0) { + fprintf(stderr, "ERROR sending netlink message\n"); + success = false; + goto cleanup; + } + DBG(printf("nl_send_auto_complete: num_bytes=%d\n", num_bytes);) + +cleanup: + if (msg != NULL) { + nlmsg_free(msg); + } + + return success; +} + +/* + * Get station info from interface via netlink + * + * This sends the request to the kernel to send us the station info, + * then waits for the response to come back. The response comes back + * as a callback (station_handler): + * + * Normal operation: + * this function sends request + * this function calls nl_recvmsgs; inside that, station_handler is called + * station_handler fills in station_info + * this function calls nl_recvmsgs; inside that, finish_handler is called + * finish_handler clears 'status' + * this function sees status is cleared and returns + * + * Almost everything returned is parsed out and available. Only a few items + * are logged, and only the signal strength is used in the mavlink message, + * but pulling them all out of the netlink message should make it easier to + * log more (or something different) if we want. + */ + +static void get_station_info(cb_ctx_t *cb_ctx, const nl_state_t *nl) +{ + memset(&cb_ctx->station_info, 0, sizeof(cb_ctx->station_info)); + + cb_ctx->status = 1; + if (!send_nlcmd(nl, NL80211_CMD_GET_STATION, NLM_F_DUMP)) { + return; + } + + /* wait for callback to set station_info and set status=0 */ + while (cb_ctx->status == 1) { + int rv = nl_recvmsgs(nl->sock, nl->cmd_cb); + if (rv != 0) { + fprintf(stderr, "nl_recvmsgs: %d\n", rv); + } + } +} + +void get_survey_info(cb_ctx_t *cb_ctx, const nl_state_t *nl) +{ + memset(&cb_ctx->survey_info, 0, sizeof(cb_ctx->survey_info)); + + cb_ctx->status = 1; + if (!send_nlcmd(nl, NL80211_CMD_GET_SURVEY, NLM_F_DUMP)) { + return; + } + + /* wait for callback to set set status=0 */ + while (cb_ctx->status == 1) { + int rv = nl_recvmsgs(nl->sock, nl->cmd_cb); + if (rv != 0) { + fprintf(stderr, "nl_recvmsgs: %d\n", rv); + } + } +} + +int valid_handler(struct nl_msg *msg, void *arg) +{ + /* + * Top level handler for all NL_CB_VALID/NL_CB_CUSTOM responses. + * + * Check the reported cmd and dispatch accordingly. + */ + + struct nlmsghdr *nl_hdr = nlmsg_hdr(msg); + struct genlmsghdr *genl_hdr = (struct genlmsghdr *)nlmsg_data(nl_hdr); + + cb_ctx_t *ctx = (cb_ctx_t *)arg; + + switch (genl_hdr->cmd) { + case NL80211_CMD_NEW_STATION: + return station_handler(msg, &ctx->station_info); + + case NL80211_CMD_NEW_SURVEY_RESULTS: + return survey_handler(msg, &ctx->survey_info); + + default: + // unexpected msg + return NL_SKIP; + } +} + +/* + * Called for each station result message that comes back from the kernel. + * We expect only one before finish_handler is called. + */ +static int station_handler(struct nl_msg *msg, station_info_t *station_info) +{ + struct nlattr *tb1[NL80211_ATTR_MAX + 1]; + struct nlmsghdr *nl_hdr; + struct genlmsghdr *genl_hdr; + struct nlattr *genl_attr_data; + int genl_attr_len; + struct nlattr *tb2[NL80211_STA_INFO_MAX + 1]; + static struct nla_policy policy2[NL80211_STA_INFO_MAX + 1] = {{0}}; + struct nlattr *tb3[NL80211_RATE_INFO_MAX + 1]; + static struct nla_policy policy3[NL80211_RATE_INFO_MAX + 1] = {{0}}; + + nl_hdr = nlmsg_hdr(msg); + genl_hdr = (struct genlmsghdr *)nlmsg_data(nl_hdr); + genl_attr_data = genlmsg_attrdata(genl_hdr, 0); + genl_attr_len = genlmsg_attrlen(genl_hdr, 0); + + if (nla_parse(tb1, NL80211_ATTR_MAX, genl_attr_data, genl_attr_len, NULL) != 0) { + fprintf(stderr, "ERROR parsing netlink message attributes\n"); + return NL_SKIP; + } + + if (tb1[NL80211_ATTR_STA_INFO] == NULL) { + printf("no data\n"); + return NL_SKIP; + } + + if (nla_parse_nested(tb2, NL80211_STA_INFO_MAX, tb1[NL80211_ATTR_STA_INFO], policy2) != 0) { + printf("ERROR parsing netlink message nested attributes\n"); + return NL_SKIP; + } + + /* Description of what attributes there are is in linux/nl80211.h */ + + /* For each possible attribute, see if it is present in the info we + * got, and if so, copy it to our station_info_t structure. */ + + if (tb2[NL80211_STA_INFO_INACTIVE_TIME] != NULL) + station_info->inactive_time = nla_get_u32(tb2[NL80211_STA_INFO_INACTIVE_TIME]); + + if (tb2[NL80211_STA_INFO_RX_BYTES] != NULL) + station_info->rx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_RX_BYTES]); + + if (tb2[NL80211_STA_INFO_TX_BYTES] != NULL) + station_info->tx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_TX_BYTES]); + + if (tb2[NL80211_STA_INFO_RX_BYTES64] != NULL) + station_info->rx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_RX_BYTES64]); + + if (tb2[NL80211_STA_INFO_TX_BYTES64] != NULL) + station_info->tx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_TX_BYTES64]); + + /* this appears to be signed dBm, not a u8 */ + if (tb2[NL80211_STA_INFO_SIGNAL] != NULL) + station_info->signal = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL]); + + /* tx_bitrate is a nested structure; the station_info points to a nested + * attribute thing that needs to be decoded */ + if (tb2[NL80211_STA_INFO_TX_BITRATE] != NULL && + nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_TX_BITRATE], policy3) == + 0) { + rate_info_t *rate_info = &station_info->tx_bitrate; + if (tb3[NL80211_RATE_INFO_BITRATE] != NULL) + rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]); + if (tb3[NL80211_RATE_INFO_MCS] != NULL) + rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]); + if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL) + rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]); + if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL) + rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]); + if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL) + rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]); + } + + if (tb2[NL80211_STA_INFO_RX_PACKETS] != NULL) + station_info->rx_packets = nla_get_u32(tb2[NL80211_STA_INFO_RX_PACKETS]); + + if (tb2[NL80211_STA_INFO_TX_PACKETS] != NULL) + station_info->tx_packets = nla_get_u32(tb2[NL80211_STA_INFO_TX_PACKETS]); + + if (tb2[NL80211_STA_INFO_TX_RETRIES] != NULL) + station_info->tx_retries = nla_get_u32(tb2[NL80211_STA_INFO_TX_RETRIES]); + + if (tb2[NL80211_STA_INFO_TX_FAILED] != NULL) + station_info->tx_failed = nla_get_u32(tb2[NL80211_STA_INFO_TX_FAILED]); + + /* this appears to be signed dBm, not a u8 */ + if (tb2[NL80211_STA_INFO_SIGNAL_AVG] != NULL) + station_info->signal_avg = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL_AVG]); + + /* rx_bitrate is nested, like tx_bitrate */ + if (tb2[NL80211_STA_INFO_RX_BITRATE] != NULL && + nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_RX_BITRATE], policy3) == + 0) { + rate_info_t *rate_info = &station_info->rx_bitrate; + if (tb3[NL80211_RATE_INFO_BITRATE] != NULL) + rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]); + if (tb3[NL80211_RATE_INFO_MCS] != NULL) + rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]); + if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL) + rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]); + if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL) + rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]); + if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL) + rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]); + } + + if (tb2[NL80211_STA_INFO_BEACON_LOSS] != NULL) + station_info->beacon_loss = nla_get_u32(tb2[NL80211_STA_INFO_BEACON_LOSS]); + + if (tb2[NL80211_STA_INFO_T_OFFSET] != NULL) + station_info->t_offset = (int64_t)nla_get_u64(tb2[NL80211_STA_INFO_T_OFFSET]); + + return NL_SKIP; + +} /* station_handler */ + +static int survey_handler(struct nl_msg *msg, survey_info_t *survey_info) +{ + /* + * https://wireless.wiki.kernel.org/en/users/Documentation/acs says + * + * - active time is the amount of time the radio has spent on the channel + * - busy time is the amount of time the channel has spent on the channel but noticed the + * channel was busy + * and could not initiate communication if it wanted to. + * - tx time is the amount of time the channel has spent on the channel transmitting data. + * - interference factor is defined as the ratio of the observed busy time over the time we + * spent on the channel, + * this value is then amplified by the noise floor observed on the channel in comparison to + * the lowest noise floor + * observed on the entire band. + */ + + struct nlattr *tb[NL80211_ATTR_MAX + 1]; + struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg)); + struct nlattr *sinfo[NL80211_SURVEY_INFO_MAX + 1]; + char dev[20]; + + static struct nla_policy survey_policy[NL80211_SURVEY_INFO_MAX + 1]; + survey_policy[NL80211_SURVEY_INFO_FREQUENCY].type = NLA_U32; + survey_policy[NL80211_SURVEY_INFO_NOISE].type = NLA_U8; + + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL); + + if_indextoname(nla_get_u32(tb[NL80211_ATTR_IFINDEX]), dev); + + if (!tb[NL80211_ATTR_SURVEY_INFO]) { + fprintf(stderr, "survey data missing!\n"); + return NL_SKIP; + } + + if (nla_parse_nested(sinfo, NL80211_SURVEY_INFO_MAX, tb[NL80211_ATTR_SURVEY_INFO], + survey_policy)) { + fprintf(stderr, "failed to parse nested attributes!\n"); + return NL_SKIP; + } + + if (sinfo[NL80211_SURVEY_INFO_FREQUENCY]) { + survey_info->in_use = sinfo[NL80211_SURVEY_INFO_IN_USE] ? true : false; + if (!survey_info->in_use) { + return NL_SKIP; + } + survey_info->freq = nla_get_u32(sinfo[NL80211_SURVEY_INFO_FREQUENCY]); + } + if (sinfo[NL80211_SURVEY_INFO_NOISE]) { + survey_info->noise = nla_get_u8(sinfo[NL80211_SURVEY_INFO_NOISE]); + } + if (sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME]) { + survey_info->chan_active_time = nla_get_u64(sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME]); + } + if (sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]) { + survey_info->chan_busy_time = nla_get_u64(sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]); + } + if (sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_EXT_BUSY]) { + survey_info->chan_ext_busy_time = + nla_get_u64(sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_EXT_BUSY]); + } + if (sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_RX]) { + survey_info->chan_receive_time = nla_get_u64(sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_RX]); + } + if (sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_TX]) { + survey_info->chan_transmit_time = nla_get_u64(sinfo[NL80211_SURVEY_INFO_CHANNEL_TIME_TX]); + } +#if 0 + if (sinfo[NL80211_SURVEY_INFO_TIME_SCAN]) { + survey_info->chan_scan_time = nla_get_u64(sinfo[NL80211_SURVEY_INFO_TIME_SCAN]); + } +#endif + + return NL_SKIP; +} + +/* Never seen this one called, but the examples have it. */ +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) +{ + + cb_ctx_t *cb_ctx = (cb_ctx_t *)arg; + cb_ctx->status = err->error; + + return NL_STOP; +} + +/* + * Called after station info message is sent. The calling of this is how we + * know we're done. + * + * Other commands sent to netlink might get several responses back; this is + * more useful in those cases. + */ +static int finish_handler(struct nl_msg *msg, void *arg) +{ + cb_ctx_t *cb_ctx = (cb_ctx_t *)arg; + cb_ctx->status = 0; + + return NL_SKIP; +} + +/* Never seen this one called, but the examples have it. */ +static int ack_handler(struct nl_msg *msg, void *arg) +{ + cb_ctx_t *cb_ctx = (cb_ctx_t *)arg; + cb_ctx->status = 0; + + return NL_STOP; +} diff --git a/flightcode/stm32/.gitignore b/flightcode/stm32/.gitignore new file mode 100644 index 0000000..0d20b64 --- /dev/null +++ b/flightcode/stm32/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/flightcode/stm32/AppConnected.h b/flightcode/stm32/AppConnected.h new file mode 100644 index 0000000..ec174fc --- /dev/null +++ b/flightcode/stm32/AppConnected.h @@ -0,0 +1,23 @@ +#ifndef APP_CONNECTED_H +#define APP_CONNECTED_H + +#include +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "PacketHandler.h" + +/*********************************************************************** +Class: The AppConnected class. + +Description: Creates a UDP socket for app-connected message data. +***********************************************************************/ +class AppConnected : public PacketHandler +{ +public: + // Constructor. Takes an IP address, port and TOS value + AppConnected(string ipaddr, int port) + : PacketHandler(ipaddr, port, IP_TOS_DEFAULT, PKT_ID_SOLO_APP_CONNECTION){}; +}; + +#endif // APP_CONNECTED_H diff --git a/flightcode/stm32/ButtonEventHandler.cpp b/flightcode/stm32/ButtonEventHandler.cpp new file mode 100644 index 0000000..b982621 --- /dev/null +++ b/flightcode/stm32/ButtonEventHandler.cpp @@ -0,0 +1,31 @@ +#include +#include "packetTypes.h" +#include "util.h" +#include "net_wmm.h" +#include "TcpServer.h" +#include "ButtonEventMessage.h" +#include "ButtonEventHandler.h" + +using namespace std; + +// Constructor - create and start the server that allows clients to connect +// and receive button events +ButtonEventHandler::ButtonEventHandler(int port) + : PacketHandler("127.0.0.1", port, 0x02, PKT_ID_BUTTON_EVENT), tcpServer(port, "ButtonEvent") +{ + tcpServer.start(); +} + +// upHandler - called when a button event message is received from the STM32. +// Prepend the timestamp and send it to all attached clients (if any). +// CLOCK_MONOTONIC is used because it seems more likely that a client would be +// interested in intervals between button events instead of absolute button +// event time. +int ButtonEventHandler::upHandler(char serBuf[], int len) +{ + uint64_t now_us; + now_us = clock_gettime_us(CLOCK_MONOTONIC); + ButtonEventMessage msg(now_us, serBuf); + tcpServer.send_clients(&msg, sizeof(msg)); + return 0; +} diff --git a/flightcode/stm32/ButtonEventHandler.h b/flightcode/stm32/ButtonEventHandler.h new file mode 100644 index 0000000..af62a30 --- /dev/null +++ b/flightcode/stm32/ButtonEventHandler.h @@ -0,0 +1,28 @@ +#ifndef BUTTON_EVENT_HANDLER_H +#define BUTTON_EVENT_HANDLER_H + +#include "PacketHandler.h" +#include "TcpServer.h" + +/*********************************************************************** +Class: The ButtonEventHandler class. + +Description: Button events are upstream-only. A TCP server is created; + a client connected to the server gets all button events + that arrive from the STM32. +***********************************************************************/ + +class ButtonEventHandler : public PacketHandler +{ + +public: + ButtonEventHandler(int port); + + int upHandler(char serBuf[], int len); + +private: + // clients attach to this to receive button events + TcpServer tcpServer; +}; + +#endif // BUTTON_EVENT_HANDLER_H diff --git a/flightcode/stm32/ButtonFunctionCfg.cpp b/flightcode/stm32/ButtonFunctionCfg.cpp new file mode 100644 index 0000000..f751025 --- /dev/null +++ b/flightcode/stm32/ButtonFunctionCfg.cpp @@ -0,0 +1,27 @@ + +#include +#include "packetTypes.h" +#include "ButtonFunctionCfg.h" + +// Output ButtonFunctionCfgMsg to a stream in a human-readable format +ostream &operator<<(ostream &os, const struct ButtonFunctionCfgMsg &msg) +{ + os << "button_id=" << int(msg.button_id); + os << " button_event=" << int(msg.button_event); + os << " shot_id=" << int(msg.shot_id); + os << " state=" << int(msg.state); + // is descriptor EOS-terminated? + os << " descriptor=\"" << msg.descriptor << '\"'; + return os; +} + +// UDP port on local machine where downstream ButtonFunctionCfg should be sent +unsigned ButtonFunctionCfg::udpPort = 0; + +ButtonFunctionCfg::ButtonFunctionCfg(int port) + : PacketHandler("127.0.0.1", port, 0x02, PKT_ID_BUTTON_FUNCTION_CFG) +{ + + ButtonFunctionCfg::udpPort = port; + +} // ButtonFunctionCfg::ButtonFunctionCfg diff --git a/flightcode/stm32/ButtonFunctionCfg.h b/flightcode/stm32/ButtonFunctionCfg.h new file mode 100644 index 0000000..5f0dfd2 --- /dev/null +++ b/flightcode/stm32/ButtonFunctionCfg.h @@ -0,0 +1,42 @@ +#ifndef BUTTON_FUNCTION_CFG_H +#define BUTTON_FUNCTION_CFG_H + +#include +#include +#include "net_wmm.h" +#include "PacketHandler.h" + +// Message as sent to the STM32; message from App is in SoloMessage.h +struct __attribute((__packed__)) ButtonFunctionCfgMsg { + // packet ID is prepended just before slip-encoding + uint8_t button_id; + uint8_t button_event; + uint8_t shot_id; + uint8_t state; + char descriptor[0]; + static const int descriptor_max = 20; // (does not appear in struct) +}; + +static const int ButtonFunctionCfgMsg_BufSize = + sizeof(ButtonFunctionCfgMsg) + ButtonFunctionCfgMsg::descriptor_max; + +ostream &operator<<(ostream &os, const struct ButtonFunctionCfgMsg &msg); + +/*********************************************************************** +Class: The ButtonFunctionCfg class. + +Description: Creates a UDP socket for ButtonFunctionCfg data. +***********************************************************************/ +class ButtonFunctionCfg : public PacketHandler +{ +public: + ButtonFunctionCfg(int port); + + // This is nasty, but I need a way to get the UDP port number to which + // downstream messages should be sent from somewhere that does not have + // access to the stm32's ButtonFunctionCfg object. This is set by the + // constructor. + static unsigned udpPort; +}; + +#endif // BUTTON_FUNCTION_CFG_H diff --git a/flightcode/stm32/CircularBuffer.cpp b/flightcode/stm32/CircularBuffer.cpp new file mode 100644 index 0000000..58f9c76 --- /dev/null +++ b/flightcode/stm32/CircularBuffer.cpp @@ -0,0 +1,10 @@ + +#include +#include "CircularBuffer.h" + +// Print CircularBuffer status to a stream (debug) +std::ostream &operator<<(std::ostream &os, const CircularBuffer &c) +{ + os << "size=" << c.size() << " used=" << c.used() << " free=" << c.free(); + return os; +} diff --git a/flightcode/stm32/CircularBuffer.h b/flightcode/stm32/CircularBuffer.h new file mode 100644 index 0000000..b599c4c --- /dev/null +++ b/flightcode/stm32/CircularBuffer.h @@ -0,0 +1,131 @@ +#ifndef CIRCULAR_BUFFER_H +#define CIRCULAR_BUFFER_H + +#include +#include +#include +#include + +// head == tail: empty +// (head - tail) == 1: one byte in buffer +// (head - tail) == (num_bytes - 1): num_bytes - 1 in buffer (full) + +class CircularBuffer +{ + +public: + // Constructor: allocate buffer and initialize state + CircularBuffer(unsigned num_bytes) : _buf(NULL), _head(0), _tail(0), _buf_size(0) + { + _buf = new (std::nothrow) char[num_bytes]; + if (_buf != NULL) + _buf_size = num_bytes; + } + + // Destructor: free buffer + ~CircularBuffer() + { + if (_buf != NULL) + delete _buf; + _buf = NULL; + _head = _tail = 0; + _buf_size = 0; + } + + bool invariant(void) + { + return (_buf != NULL) && (_buf_size > 0) && (_head < _buf_size) && (_tail < _buf_size); + } + + // Return size + unsigned size(void) const + { + return _buf_size; + } + + // Return number of bytes used in buffer. + unsigned used(void) const + { + if (_head >= _tail) + return _head - _tail; + else + return _head + _buf_size - _tail; + } + + // Return number of free bytes in buffer. + unsigned free(void) const + { + return _buf_size - used() - 1; + } + + // Put data in buffer. + // Return true on success, false on error. + bool put(const void *p, unsigned num_bytes) + { + // std::cout << "CircularBuffer::put(" << p << ", " << num_bytes + // << ")" << std::endl; + if (num_bytes > free()) + return false; + const char *d = (const char *)p; + while (num_bytes-- > 0) { + _buf[_head++] = *d++; + if (_head >= _buf_size) + _head = 0; + } + return true; + } + + // Put structure in buffer. + // Return true on success, false on error. + template < class type > + bool put(const type p) + { + return put(&p, sizeof(type)); + } + + // Get data from buffer. + // Return true on success, false on error. + bool get(void *p, unsigned num_bytes) + { + if (used() < num_bytes) + return false; + char *d = (char *)p; + while (num_bytes-- > 0) { + *d++ = _buf[_tail++]; + if (_tail >= _buf_size) + _tail = 0; + } + return true; + } + + // Get structure from buffer. + // Return true on success, false on error. + template < class type > + bool get(type *p) + { + return get(p, sizeof(type)); + } + + // Discard data from buffer. + // Return true on success, false on error. + bool discard(unsigned num_bytes) + { + if (used() < num_bytes) + return false; + while (num_bytes-- > 0) { + if (++_tail >= _buf_size) + _tail = 0; + } + return true; + } + +private: + char *_buf; + unsigned _head; + unsigned _tail; + unsigned _buf_size; +}; + +std::ostream &operator<<(std::ostream &os, const CircularBuffer &c); + +#endif // CIRCULAR_BUFFER_H diff --git a/flightcode/stm32/CircularBufferTest.cpp b/flightcode/stm32/CircularBufferTest.cpp new file mode 100644 index 0000000..e2ac569 --- /dev/null +++ b/flightcode/stm32/CircularBufferTest.cpp @@ -0,0 +1,167 @@ + +#include +#include "CircularBuffer.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + static const unsigned size = 16; + CircularBuffer cb(size); + bool b[10]; + uint32_t u32[10]; + uint64_t u64[10]; + char c[10]; + + cout << cb << endl; + + cout << "put uint32_t..."; + u32[0] = 0x12345678; + b[0] = cb.put(u32[0]); + if (b[0] && cb.used() == 4 && cb.free() == cb.size() - 4 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get uint32_t..."; + cb.get(&u32[1]); + if (b[0] && u32[1] == u32[0] && cb.used() == 0 && cb.free() == cb.size() - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put 3x uint32_t..."; + u32[0] = 0x11223344; + u32[1] = 0x55667788; + u32[2] = 0x99aabbcc; + b[0] = cb.put(u32[0]); + b[1] = cb.put(u32[1]); + b[2] = cb.put(u32[2]); + if (b[0] && b[1] && b[2] && cb.used() == 12 && cb.free() == cb.size() - 12 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put uint32_t (fail)..."; + u32[3] = 0xdeadbeef; + b[3] = cb.put(u32[3]); + if (!b[3] && cb.used() == 12 && cb.free() == cb.size() - 12 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put 3x char..."; + c[0] = 'a'; + c[1] = 'b'; + c[2] = 'c'; + b[0] = cb.put(c[0]); + b[1] = cb.put(c[1]); + b[2] = cb.put(c[2]); + if (b[0] && b[1] && b[2] && cb.used() == 15 && cb.free() == cb.size() - 15 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put char (fail)..."; + c[3] = 'x'; + b[3] = cb.put(c[3]); + if (!b[3] && cb.used() == 15 && cb.free() == cb.size() - 15 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get 3x uint32_t..."; + u32[0] = 0; + u32[1] = 0; + u32[2] = 0; + b[0] = cb.get(&u32[0]); + b[1] = cb.get(&u32[1]); + b[2] = cb.get(&u32[2]); + if (b[0] && b[1] && b[2] && u32[0] == 0x11223344 && u32[1] == 0x55667788 && + u32[2] == 0x99aabbcc && cb.used() == 3 && cb.free() == cb.size() - 3 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get 3x char..."; + c[0] = 0; + c[1] = 0; + c[2] = 0; + b[0] = cb.get(&c[0]); + b[1] = cb.get(&c[1]); + b[2] = cb.get(&c[2]); + if (b[0] && b[1] && b[2] && c[0] == 'a' && c[1] == 'b' && c[2] == 'c' && cb.used() == 0 && + cb.free() == cb.size() - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put char..."; + c[0] = 'z'; + b[0] = cb.put(c[0]); + if (b[0] && cb.used() == 1 && cb.free() == cb.size() - 1 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "put uint64_t..."; + u64[0] = 0x123456789abcdef0ULL; + b[0] = cb.put(u64[0]); + if (b[0] && cb.used() == 9 && cb.free() == cb.size() - 9 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get char..."; + c[0] = 0; + b[0] = cb.get(&c[0]); + if (b[0] && c[0] == 'z' && cb.used() == 8 && cb.free() == cb.size() - 8 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get uint64_t..."; + u64[0] = 0; + b[0] = cb.get(&u64[0]); + if (b[0] && u64[0] == 0x123456789abcdef0ULL && cb.used() == 0 && cb.free() == cb.size() - 0 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + cout << "get char (fail)..."; + c[0] = 0; + b[0] = cb.get(&c[0]); + if (!b[0] && cb.used() == 0 && cb.free() == cb.size() - 0 - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + + cout << cb << endl; + + return 0; + +} // main diff --git a/flightcode/stm32/ConfigStickAxes.cpp b/flightcode/stm32/ConfigStickAxes.cpp new file mode 100644 index 0000000..d7e7624 --- /dev/null +++ b/flightcode/stm32/ConfigStickAxes.cpp @@ -0,0 +1,11 @@ + +#include "packetTypes.h" +#include "ConfigStickAxes.h" + +// UDP port on local machine where downstream ConfigStickAxesMsg should be sent +unsigned ConfigStickAxes::udpPort = 0; + +ConfigStickAxes::ConfigStickAxes(int port) : PacketHandler("", port, 0x02, PKT_ID_CONFIG_STICK_AXES) +{ + ConfigStickAxes::udpPort = port; +} diff --git a/flightcode/stm32/ConfigStickAxes.h b/flightcode/stm32/ConfigStickAxes.h new file mode 100644 index 0000000..eec3cad --- /dev/null +++ b/flightcode/stm32/ConfigStickAxes.h @@ -0,0 +1,14 @@ +#ifndef CONFIG_STICK_AXES_H +#define CONFIG_STICK_AXES_H + +#include "PacketHandler.h" + +class ConfigStickAxes : public PacketHandler +{ +public: + ConfigStickAxes(int port); + + static unsigned udpPort; +}; + +#endif // CONFIG_STICK_AXES_H diff --git a/flightcode/stm32/ConfigSweepTime.cpp b/flightcode/stm32/ConfigSweepTime.cpp new file mode 100644 index 0000000..e11a56b --- /dev/null +++ b/flightcode/stm32/ConfigSweepTime.cpp @@ -0,0 +1,11 @@ + +#include "packetTypes.h" +#include "ConfigSweepTime.h" + +// UDP port on local machine where downstream ConfigSweepTime should be sent +unsigned ConfigSweepTime::udpPort = 0; + +ConfigSweepTime::ConfigSweepTime(int port) : PacketHandler("", port, 0x02, PKT_ID_CONFIG_SWEEP_TIME) +{ + ConfigSweepTime::udpPort = port; +} diff --git a/flightcode/stm32/ConfigSweepTime.h b/flightcode/stm32/ConfigSweepTime.h new file mode 100644 index 0000000..479c9f4 --- /dev/null +++ b/flightcode/stm32/ConfigSweepTime.h @@ -0,0 +1,14 @@ +#ifndef CONFIG_SWEEP_TIME_H +#define CONFIG_SWEEP_TIME_H + +#include "PacketHandler.h" + +class ConfigSweepTime : public PacketHandler +{ +public: + ConfigSweepTime(int port); + + static unsigned udpPort; +}; + +#endif // CONFIG_SWEEP_TIME_H diff --git a/flightcode/stm32/InputReport.cpp b/flightcode/stm32/InputReport.cpp new file mode 100644 index 0000000..d18c027 --- /dev/null +++ b/flightcode/stm32/InputReport.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include +#include +#include +#include "packetTypes.h" +#include "util.h" +#include "net_wmm.h" +#include "TcpServer.h" +#include "InputReport.h" + +using namespace std; + +// Send an InputReport to a stream in a friendly format. +ostream &operator<<(ostream &os, const struct InputReportMessage &msg) +{ + os << "msgId=" << msg.msgId; + os << " length=" << msg.length; + os << " timestamp=" << msg.timestamp; + os << " gimbalY=" << msg.gimbalY; + os << " gimbalRate=" << msg.gimbalRate; + os << " battery=" << msg.battery; + return os; +} + +// Constructor - create and start the server that allows clients to connect +// and receive input reports +InputReport::InputReport(int port, int msg_rate) + : PacketHandler("127.0.0.1", port, IP_TOS_DEFAULT, PKT_ID_INPUT_REPORT), + tcpServer(port, "InputReport"), msgInterval_us(1000000 / msg_rate), nextMsgTime_us(0) +{ + tcpServer.start(); +} + +// upHandler - called when an inpurt report message is received from the +// STM32. Prepend the timestamp and send it to all attached clients (if any). +// The timestamp is include in case it is useful for debug/analysis, and +// CLOCK_MONOTONIC is used because it seems more likely that message intervals +// are more interesting that absolute times. +int InputReport::upHandler(char serBuf[], int len) +{ + uint64_t now_us; + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (now_us > nextMsgTime_us) { + InputReportMessage msg(now_us, serBuf); + tcpServer.send_clients(&msg, sizeof(msg)); + // the following assumes CLOCK_MONOTONIC (no jumps) + if (nextMsgTime_us == 0) + nextMsgTime_us = now_us; + nextMsgTime_us += msgInterval_us; + } + return 0; +} diff --git a/flightcode/stm32/InputReport.h b/flightcode/stm32/InputReport.h new file mode 100644 index 0000000..3266eda --- /dev/null +++ b/flightcode/stm32/InputReport.h @@ -0,0 +1,64 @@ +#ifndef INPUT_REPORT_H +#define INPUT_REPORT_H + +#include +#include +#include "PacketHandler.h" +#include "TcpServer.h" + +// The message as sent to clients connected to the TCP server +struct __attribute((__packed__)) InputReportMessage { + // These fields are expected to be the only data and in this order. + // The message constructor takes the raw message from the STM32 and picks + // out fields to fill these in. + uint32_t msgId; + uint32_t length; // bytes following, i.e. 16 + uint64_t timestamp; + uint16_t gimbalY; + uint16_t gimbalRate; + uint16_t battery; + uint16_t spare; // to make it a multiple of 4 + + static const uint32_t MSG_ID = 2003; // XXX move elsewhere + + InputReportMessage(uint64_t ts, const char *rawMsg) + : msgId(MSG_ID), length(sizeof(InputReportMessage) - 8), timestamp(ts), spare(0) + { + // rawMsg is as produced in artoo's Inputs::producePacket() (without + // the initial packet ID). All fields little endian. + gimbalY = rawMsg[0] | ((uint16_t)(rawMsg[1]) << 8); + gimbalRate = rawMsg[2] | ((uint16_t)(rawMsg[3]) << 8); + battery = rawMsg[4] | ((uint16_t)(rawMsg[5]) << 8); + } +}; + +/*********************************************************************** +Class: The InputReport class. + +Description: Input reports are upstream-only. A TCP server is created; + a client connected to the server gets input reports that + arrive from the STM32. The constructor's 'msg_rate' + determines how many messages per second are forwarded. + Messages come from the STM32 at 50 Hz, but for battery + monitoring, the forwarding rate can be set much lower + (e.g. 1 Hz). +***********************************************************************/ + +class InputReport : public PacketHandler +{ + +public: + InputReport(int port, int msg_rate); + + int upHandler(char serBuf[], int len); + +private: + // clients attach to this to receive input reports + TcpServer tcpServer; + + // message rate limiting + unsigned msgInterval_us; + uint64_t nextMsgTime_us; +}; + +#endif // INPUT_REPORT_H diff --git a/flightcode/stm32/LockoutState.cpp b/flightcode/stm32/LockoutState.cpp new file mode 100644 index 0000000..f79044d --- /dev/null +++ b/flightcode/stm32/LockoutState.cpp @@ -0,0 +1,20 @@ + +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "LockoutState.h" + +// Output LockoutState to a stream in a human-readable format +ostream &operator<<(ostream &os, const struct LockoutStateMsg &msg) +{ + os << "lockout=" << (msg.lockout ? "true" : "false"); + return os; +} + +// UDP port on local machine where downstream LockoutState should be sent +unsigned LockoutState::udpPort = 0; + +LockoutState::LockoutState(int port) : PacketHandler("127.0.0.1", port, 0x02, PKT_ID_LOCKOUT_STATE) +{ + LockoutState::udpPort = port; +} diff --git a/flightcode/stm32/LockoutState.h b/flightcode/stm32/LockoutState.h new file mode 100644 index 0000000..48dbc1d --- /dev/null +++ b/flightcode/stm32/LockoutState.h @@ -0,0 +1,29 @@ +#ifndef LOCKOUT_STATE_H +#define LOCKOUT_STATE_H + +#include +#include +#include "PacketHandler.h" + +// Message as sent to the STM32 +struct __attribute((__packed__)) LockoutStateMsg { + // packet ID is prepended just before slip-encoding + uint8_t lockout; // nonzero for lockout, zero for not locked out +}; + +ostream &operator<<(ostream &os, const struct LockoutStateMsg &msg); + +/*********************************************************************** +Class: The LockoutState class. + +Description: Creates a UDP socket for LockoutState data. +***********************************************************************/ +class LockoutState : public PacketHandler +{ +public: + LockoutState(int port); + + static unsigned udpPort; +}; + +#endif // LOCKOUT_STATE_H diff --git a/flightcode/stm32/Makefile b/flightcode/stm32/Makefile new file mode 100644 index 0000000..c684476 --- /dev/null +++ b/flightcode/stm32/Makefile @@ -0,0 +1,75 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../ini ../ini/cpp + +INCS = -I../util -I../ini -I../ini/cpp + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS = -lpthread + +SRCS_CPP = stm32.cpp +SRCS_CPP += PacketHandler.cpp +SRCS_CPP += PairReq.cpp +SRCS_CPP += RC.cpp +SRCS_CPP += SLIP.cpp +SRCS_CPP += SysInfo.cpp +SRCS_CPP += Telem.cpp +SRCS_CPP += ButtonEventMessage.cpp +SRCS_CPP += ButtonEventHandler.cpp +SRCS_CPP += InputReport.cpp +SRCS_CPP += ButtonFunctionCfg.cpp +SRCS_CPP += SetShotInfo.cpp +SRCS_CPP += TcpServer.cpp +SRCS_CPP += SoloMessage.cpp +SRCS_CPP += INIReader.cpp +SRCS_CPP += SerialLog.cpp +SRCS_CPP += CircularBuffer.cpp +SRCS_CPP += LockoutState.cpp +SRCS_CPP += ConfigStickAxes.cpp +SRCS_CPP += SetTelemUnits.cpp +SRCS_CPP += ConfigSweepTime.cpp +SRCS_C = ini.c +SRCS_C += util.c +SRCS_C += mutex.c +SRCS_C += syslog.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = stm32 + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +# This has run on a Linux host +CircularBufferTest: CircularBufferTest.o CircularBuffer.o + $(LINK.cpp) -o CircularBufferTest CircularBufferTest.o CircularBuffer.o + +# This has run on a Linux host +SerialLogTest: SerialLogTest.o SerialLog.o CircularBuffer.o util.o + $(LINK.cpp) -o SerialLogTest SerialLogTest.o SerialLog.o CircularBuffer.o util.o + +clean: + $(RM) *.o *~ $(MAIN) *Test + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/stm32/PacketHandler.cpp b/flightcode/stm32/PacketHandler.cpp new file mode 100644 index 0000000..0f31013 --- /dev/null +++ b/flightcode/stm32/PacketHandler.cpp @@ -0,0 +1,119 @@ +#include +#include +#include +#include +#include +#include +#include +#include "PacketHandler.h" +#include "SLIP.h" +#include +#include "SerialLog.h" + +using namespace std; + +#define BUFSIZE 4096 + +/*********************************************************************** +Method: PacketHandler constructor + +Description: Sets up the PacketHandler UDP port based on an ip address string, + port, TOS value, and pktID type. + ***********************************************************************/ +PacketHandler::PacketHandler(string ipaddr, int port, int tos, int pktID) : _pktID(pktID) +{ + // TODO: Handle a failure of opening the socket + // TODO: put a timeout on this port in case the select messes up + + /* create a UDP socket */ + if ((_sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + syslog(LOG_ERR, "pkt: cannot create socket"); + return; + } + + /* bind the socket to any valid IP address and a specific port */ + memset((char *)&_sock, 0, sizeof(_sock)); + _sock.sin_family = AF_INET; + + if (ipaddr != "") // If its empty, we let the overriding methods handle it + inet_aton(ipaddr.c_str(), &_sock.sin_addr); + + _sock.sin_port = htons(port); + + if (bind(_sock_fd, (struct sockaddr *)&_sock, sizeof(_sock)) < 0) { + syslog(LOG_ERR, "pkt: bind failed on port %d", port); + return; + } + + setsockopt(_sock_fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)); + + syslog(LOG_INFO, "pkt: opened port %d", port); +} + +int PacketHandler::upHandler(char serBuf[], int len) +{ + int bytesSent; + + // We should have already stripped the pktID and decoded + // the data, so send it along! + // Ignore any errors. + bytesSent = sendto(_sock_fd, serBuf, len, 0, (struct sockaddr *)&_sock, sizeof(_sock)); + + if (bytesSent > 0) + return bytesSent; + + return 0; +} + +void PacketHandler::downHandler(int ser_fd, uint32_t debug) +{ + char buf[BUFSIZE]; + socklen_t addrlen = sizeof(_sock); + int recvlen; + char msg[1024]; + int encodedLen; + // downHandler() only called in the context of stm32.cpp::downstream_task + // so a single instance of slipEnc is okay + static SLIPEncoder *slipEnc = new SLIPEncoder(msg, sizeof(msg)); + + // First byte is the data type. This will be + // left alone by the SLIP encoding, so its ok to put it in now + buf[0] = _pktID; + + // Attempt to receive data. This should be good since we got here from + // a select() + recvlen = recvfrom(_sock_fd, &buf[1], BUFSIZE, 0, (struct sockaddr *)&_sock, &addrlen); + +#ifdef INCLUDE_SERIAL_LOG + extern SerialLog *serialLog; + serialLog->log_packet(buf, recvlen + 1, SerialLog::PKTFLG_DOWN); +#endif // INCLUDE_SERIAL_LOG + + // Pack this data with slip encoding and dump it down to the STM32 + // Remember that we prepended the packet type + encodedLen = slipEnc->encode(buf, recvlen + 1); + + if (debug & (1 << _pktID)) { + char buf[200]; + char *p = buf; + int m = sizeof(buf) - 1; + + memset(buf, 0, sizeof(buf)); + + for (int i = 0; i < encodedLen; i++) { + int n = snprintf(p, m, "%02x ", (unsigned)(msg[i])); + if (n >= m) + break; + p += n; + m -= n; + } + + syslog(LOG_INFO, "%s", buf); + } + + if (encodedLen < 0) + syslog(LOG_ERR, "pkt: slip error"); + + if (write(ser_fd, msg, encodedLen) != encodedLen) + syslog(LOG_ERR, "pkt: could not write to serial port"); +} diff --git a/flightcode/stm32/PacketHandler.h b/flightcode/stm32/PacketHandler.h new file mode 100644 index 0000000..b388f6f --- /dev/null +++ b/flightcode/stm32/PacketHandler.h @@ -0,0 +1,44 @@ +#ifndef _PACKETHANDLER_H_ +#define _PACKETHANDLER_H_ + +#include +#include +#include + +using namespace std; + +/*********************************************************************** +Class: The PacketHandler class. + +Description: Creates a UDP socket for data, which is sent + bidirectionally to and from the stm32. This is the base + class for all types of data handled by the stm32 +***********************************************************************/ + +class PacketHandler +{ +protected: + // Socket descriptor and the socket address + int _sock_fd; + struct sockaddr_in _sock; + int _pktID; + +public: + // Constructor. Takes an IP address, port, TOS value + // and the packet id int + PacketHandler(string ipaddr, int port, int tos, int pktID); + + // Handles upstream data transmission, returns the number of bytes sent + int upHandler(char serBuf[], int len); + + // Handles downstream data transmission + void downHandler(int ser_fd, uint32_t debug = 0); + + // The upstream thread needs to get the file descriptor + inline int getfd(void) + { + return _sock_fd; + }; +}; + +#endif //_PACKETHANDLER_H diff --git a/flightcode/stm32/PairReq.cpp b/flightcode/stm32/PairReq.cpp new file mode 100644 index 0000000..a649a0a --- /dev/null +++ b/flightcode/stm32/PairReq.cpp @@ -0,0 +1,29 @@ +#include +#include +#include +#include +#include "PairReq.h" + +using namespace std; + +// This is really a pair confirm message and should be a separate handler from +// pair request (down is pair request, up is pair confirm, but with different +// packet IDs). + +int PairReq::upHandler(char serBuf[], int len) +{ + int nBytes; + + syslog(LOG_INFO, "pair confirm going up for \"%s\"", serBuf); + + nBytes = strlen(serBuf) + 1; + + nBytes = sendto(_sock_fd, serBuf, nBytes, 0, (struct sockaddr *)&_sock, sizeof(_sock)); + + if (nBytes == -1) { + syslog(LOG_ERR, "PairReq: sendto failed"); + nBytes = 0; + } + + return nBytes; +} diff --git a/flightcode/stm32/PairReq.h b/flightcode/stm32/PairReq.h new file mode 100644 index 0000000..8cacc68 --- /dev/null +++ b/flightcode/stm32/PairReq.h @@ -0,0 +1,26 @@ +#ifndef _PAIRREQ_H_ +#define _PAIRREQ_H_ + +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "PacketHandler.h" + +using namespace std; + +/*********************************************************************** +Class: The pairReq class. + +Description: Creates a UDP socket for pairReqest data. +***********************************************************************/ +class PairReq : public PacketHandler +{ +public: + // Constructor. Takes an IP address, port and TOS value + PairReq(string ipaddr, int port) : PacketHandler(ipaddr, port, 0x02, PKT_ID_PAIR_REQUEST){}; + + // Handles upstream data transmission + int upHandler(char serBuf[], int len); +}; + +#endif //_PAIRREQ_H diff --git a/flightcode/stm32/PairRes.h b/flightcode/stm32/PairRes.h new file mode 100644 index 0000000..d27d65b --- /dev/null +++ b/flightcode/stm32/PairRes.h @@ -0,0 +1,23 @@ +#ifndef _PAIRRES_H_ +#define _PAIRRES_H_ + +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "PacketHandler.h" + +using namespace std; + +/*********************************************************************** +Class: The PairRes class. + +Description: Creates a UDP socket for pairRes data. +***********************************************************************/ +class PairRes : public PacketHandler +{ +public: + // Constructor. Takes an IP address, port and TOS value + PairRes(string ipaddr, int port) : PacketHandler(ipaddr, port, 0x02, PKT_ID_PAIR_RESULT){}; +}; + +#endif //_PAIRRES_H diff --git a/flightcode/stm32/ParamStoredVals.h b/flightcode/stm32/ParamStoredVals.h new file mode 100644 index 0000000..a5acfa8 --- /dev/null +++ b/flightcode/stm32/ParamStoredVals.h @@ -0,0 +1,13 @@ +#ifndef PARAM_STORED_VALS_H +#define PARAM_STORED_VALS_H + +#include "PacketHandler.h" +#include "packetTypes.h" + +class ParamStoredVals : public PacketHandler +{ +public: + ParamStoredVals(int port) : PacketHandler("", port, 0x02, PKT_ID_PARAM_STORED_VALS){}; +}; + +#endif // PARAM_STORED_VALS_H diff --git a/flightcode/stm32/RC.cpp b/flightcode/stm32/RC.cpp new file mode 100644 index 0000000..3e845a4 --- /dev/null +++ b/flightcode/stm32/RC.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "packetTypes.h" +#include "util.h" +#include "RC.h" + +#define RC_BUFSIZE 26 + +/* How often to log a sendto() error */ +#define SENDTO_LOG_DT_US 1000000 + +using namespace std; + +/*********************************************************************** +Method: RC constructor + +Description: Sets up the RC UDP port based on an ip address string, port + and TOS value. + Call the base constructor, but store the solo IP address + separately as we may not be connected to it at the moment +***********************************************************************/ +RC::RC(string ipaddr, int port, int tos) : PacketHandler("", port, tos, PKT_ID_DSM_CHANNELS) +{ + // Set up the sequence number + _sequence = 0; + + // We are not yet connected + _connected = false; +} + +/*********************************************************************** +Method: RC message constructor + +Description: Encodes the RC data in the following format: + +he packet as received has the channel data starting at pkt[0]. +Channel data is little-endian. + + Start + Byte Size Description + 0 2 Channel 0 + 2 2 Channel 1 + 4 2 Channel 2 + 6 2 Channel 3 + 8 2 Channel 4 + 10 2 Channel 5 + 12 2 Channel 6 + 14 2 Channel 7 + 16 (packet length) + + A packet as sent to a UDP port has the following format. All fields + are little-endian. + + Start + Byte Size Description + 0 8 Timestamp, usec since stm32 process started + 8 2 Sequence number + 10 2 Channel 0 + 12 2 Channel 1 + 14 2 Channel 2 + 16 2 Channel 3 + 18 2 Channel 4 + 20 2 Channel 5 + 22 2 Channel 6 + 24 2 Channel 7 + 26 (packet length) + +***********************************************************************/ +int RC::constructMessage(char inBuf[], char outBuf[], int inBufLen) +{ + uint64_t timestamp_us; + + // Note that this is CLOCK_REALTIME, vs. CLOCK_MONOTONIC used + // in most other places. + timestamp_us = clock_gettime_us(CLOCK_REALTIME); + + memcpy(outBuf, ×tamp_us, sizeof(timestamp_us)); + memcpy(&outBuf[8], &_sequence, sizeof(_sequence)); + memcpy(&outBuf[10], inBuf, inBufLen); + + ++_sequence; + + return (inBufLen + 10); +} + +/*********************************************************************** +Method: RC data handler + +Description: Transmits RC data over the UDP. Checks RC data for + integrity, and logs if incorrect +***********************************************************************/ +int RC::upHandler(char serBuf[], int len) +{ + char buf[RC_BUFSIZE]; + int n; + int bytesSent; + static uint64_t last_logerr_us = 0; + uint64_t now_us; + static int n_sendto_errs = 0; + + // Need to wait until we get the solo IP address from the + // pair server before sending any RC data. + if (!_connected) + return 0; + + if (len != 16) { + // this happens! + int npr = 16; + if (npr > len) + npr = len; + char buf[64]; + char *p = buf; + int m = sizeof(buf) - 1; + memset(buf, 0, sizeof(buf)); + for (int i = 0; i < npr; i++) { + int n = snprintf(p, m, "%02x ", (unsigned)(serBuf[i])); + if (n >= m) + break; + p += n; + m -= n; + } + syslog(LOG_ERR, "bad RC packet (len=%d): %s", len, buf); + return 0; + } + + // Create the string to be sent + n = constructMessage(serBuf, buf, len); + + // Send the data + bytesSent = sendto(_sock_fd, buf, n, 0, (struct sockaddr *)&_sock, sizeof(_sock)); + + if (bytesSent <= 0) { + ++n_sendto_errs; + + // Log the error and the number of failures we've seen + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (now_us - last_logerr_us > SENDTO_LOG_DT_US) { + syslog(LOG_ERR, "RC send failed (%i): %s\n", n_sendto_errs, strerror(errno)); + last_logerr_us = now_us; + n_sendto_errs = 0; + } + + return 0; + } + + return bytesSent; +} + +/*********************************************************************** +Method: setSoloIP + +Description: Sets the Solo IP after receiving it from the solo ip file +***********************************************************************/ +void RC::setSoloIP(string *address) +{ + // Changing the socket is thread-safe because we check the + //_connected flag in the upHandler. + inet_aton(address->c_str(), &_sock.sin_addr); + _connected = true; +} diff --git a/flightcode/stm32/RC.h b/flightcode/stm32/RC.h new file mode 100644 index 0000000..0304ce0 --- /dev/null +++ b/flightcode/stm32/RC.h @@ -0,0 +1,42 @@ +#ifndef _RC_H_ +#define _RC_H_ + +#include +#include +#include +#include "PacketHandler.h" + +using namespace std; + +/*********************************************************************** +Class: The RC class. + +Description: The RC class. Creates a UDP socket for RC channel + transmission and handles all channel data encoding. +***********************************************************************/ + +class RC : public PacketHandler +{ +private: + // Packs the channel data from the STM32 with a timestamp + // and sequence which is sent to the receiver on the Solo. + int constructMessage(char inBuf[], char outBuf[], int inBufLen); + + // RC packet information + int32_t _sequence; + + // If we're connected to the Solo yet. + bool _connected; + +public: + // Constructor. Takes an IP address, port and TOS value + RC(string ipaddr, int port, int tos); + + // Override the uphandler + int upHandler(char serBuf[], int len); + + // If we've got a new Solo IP address + void setSoloIP(string *address); +}; + +#endif //_RC_H diff --git a/flightcode/stm32/SLIP.cpp b/flightcode/stm32/SLIP.cpp new file mode 100644 index 0000000..6c82518 --- /dev/null +++ b/flightcode/stm32/SLIP.cpp @@ -0,0 +1,124 @@ +#include +#include "SLIP.h" + +using namespace std; + +SLIP::SLIP(char *msg, int maxMsgLen) + : _msg(msg), _msgPtr(msg), _maxLen(maxMsgLen), _msgLen(0), _escTriggered(false) +{ +} + +/**************************************************************** +Class: SLIPEncoder + +Description: Encodes a byte stream into SLIP +****************************************************************/ +SLIPEncoder::SLIPEncoder(char *msg, int maxMsgLen) : SLIP(msg, maxMsgLen){}; + +/**************************************************************** +Method: encode(char *buf, int bufLen) + +Description: Encodes a buffer into SLIP, returns the length of + the encoded message +****************************************************************/ +int SLIPEncoder::encode(char *buf, int bufLen) // Encodes an array of bytes +{ + int ret = -1; + int i; + + // Prepend with an end + *_msgPtr++ = END; + _msgLen++; + + for (i = 0; i < bufLen; ++i) { + // Add the esc sequence if necessary + if (buf[i] == ESC || buf[i] == END) { + *_msgPtr++ = ESC; + *_msgPtr++ = (buf[i] == ESC ? ESC_ESC : ESC_END); + _msgLen += 2; + } + // Otherwise just copy bytes + else { + *_msgPtr++ = buf[i]; + ++_msgLen; + } + + // Error checking + if (_msgLen > _maxLen) { + reset(); + return -1; + } + } + + // Add an END at the end...novel idea. + *_msgPtr = END; + ++_msgLen; + + ret = _msgLen; + reset(); + + return ret; +} + +/**************************************************************** +Class: SLIPDecoder + +Description: Accepts encoded SLIP bytes in and creates a decoded + message. +****************************************************************/ +SLIPDecoder::SLIPDecoder(char *msg, int maxMsgLen) : SLIP(msg, maxMsgLen){}; + +/**************************************************************** +Method: addByte(char *b) + +Description: Adds bytes from an encoded SLIP message to an + decoded SLIP message. Returns a positive value when + the entire message has been read, zero if not done. + A negative number indicates an error. +****************************************************************/ +int SLIPDecoder::addByte(char *b) +{ + int retVal = -1; + + // and END packet indicates the end of a message + if (*b == END) { + retVal = _msgLen; + + // Reset the _msgLen for the next message + reset(); + + return retVal; + } + + // Error check + if (_msgLen > _maxLen) { + reset(); + return -1; + } + + // Otherwise, append the byte to the message. + + // Check for ESC and END bytes + if (_escTriggered) { + // This should be an ESC_END or ESC_ESC + if (*b != ESC_END && *b != ESC_ESC) { + reset(); + return -1; + } else { + *_msgPtr++ = (*b == ESC_END ? END : ESC); + ++_msgLen; + } + + _escTriggered = false; + } else { + // Look for an escape sequence + if (*b == ESC) { + _escTriggered = true; + } else { + *_msgPtr++ = *b; + ++_msgLen; + } + } + + return 0; +} diff --git a/flightcode/stm32/SLIP.h b/flightcode/stm32/SLIP.h new file mode 100644 index 0000000..879b66d --- /dev/null +++ b/flightcode/stm32/SLIP.h @@ -0,0 +1,42 @@ +#ifndef _SLIP_H +#define _SLIP_H + +class SLIP +{ +public: + SLIP(char *msg, int maxMsgLen); + +protected: + char *_msg; // Pointer to the message bufer + char *_msgPtr; // The current read/write pointer + int _maxLen; + int _msgLen; + bool _escTriggered; + static const unsigned END = 0300; // indicates end of packet + static const unsigned ESC = 0333; // indicates byte stuffing + static const unsigned ESC_END = 0334; // ESC ESC_END means END data byte + static const unsigned ESC_ESC = 0335; // ESC ESC_ESC means ESC data byte + + void reset(void) + { + _msgPtr = _msg; + _msgLen = 0; + _escTriggered = false; + }; +}; + +class SLIPDecoder : public SLIP +{ +public: + SLIPDecoder(char *msg, int maxMsgLen); + int addByte(char *b); // Returns -1: error, 0:no message yet, >0:done, msg available +}; + +class SLIPEncoder : public SLIP +{ +public: + SLIPEncoder(char *msg, int maxMsgLen); + int encode(char *buf, int bufLen); // Encodes an array of bytes, returns len +}; + +#endif //_SLIP_H diff --git a/flightcode/stm32/SerialLog.cpp b/flightcode/stm32/SerialLog.cpp new file mode 100644 index 0000000..2e43d60 --- /dev/null +++ b/flightcode/stm32/SerialLog.cpp @@ -0,0 +1,198 @@ + +#include +#include +#include +#include +#include "util.h" +#include "SerialLog.h" + +SerialLog::SerialLog(unsigned log_size, unsigned timeout) + : _c_buf(log_size), _mutex_initted(false), _timeout_us(timeout), _last_up_time_us(0), + _enabled(false) +{ + + if ((_c_buf.free() > 0) && (pthread_mutex_init(&_mutex, NULL) == 0)) { + _mutex_initted = true; + _enabled = true; + } + +} // SerialLog::SerialLog + +SerialLog::~SerialLog() +{ + if (_mutex_initted) { + pthread_mutex_destroy(&_mutex); + _mutex_initted = false; + } + _enabled = false; + +} // SerialLog::~SerialLog() + +// Discard packet at tail. +// Assume already locked. If the log is corrupt, e.g. there is enough data for +// PacketEntry but not enough for the packet data, then this won't work (but +// it is already corrupt). +bool SerialLog::discard_packet(void) +{ + PacketEntry pe; + + // get PacketEntry + if (!_c_buf.get(&pe)) + return false; + + // discard variable-length packet data + return _c_buf.discard(pe.num_bytes); + +} // SerialLog::discard_packet + +void SerialLog::check_timeout(uint64_t now) +{ + + if (_enabled && (_last_up_time_us != 0) && (_timeout_us != 0) && + ((now - _last_up_time_us) > _timeout_us)) { + _enabled = false; + } + +} // SerialLog::check_timeout + +bool SerialLog::log_packet(const void *packet, unsigned num_bytes, uint8_t flags) +{ + + lock(); + + // Timestamp logged is CLOCK_REALTIME. It is believed this is more + // useful when looking at logs. + + // Timestamp for timeout detection is CLOCK_MONOTONIC. This is required + // to avoid false triggers (or missed triggers) when the time changes. + + uint64_t now_mt = clock_gettime_us(CLOCK_MONOTONIC); + uint64_t now_rt = clock_gettime_us(CLOCK_REALTIME); + + check_timeout(now_mt); + + if (!_enabled) { + unlock(); + return false; + } + + // discard from the tail until there is room + unsigned total_bytes = sizeof(PacketEntry) + num_bytes; + while (_c_buf.free() < total_bytes) { + if (!discard_packet()) { + unlock(); + return false; + } + } + + PacketEntry pe; + pe.timestamp = now_rt; + pe.num_bytes = num_bytes; + pe.flags = flags; + _c_buf.put(pe); + + _c_buf.put(packet, num_bytes); + + if ((flags & PKTFLG_DIR) == PKTFLG_UP) + _last_up_time_us = now_mt; + + unlock(); + + return true; + +} // SerialLog::log_packet + +std::ostream &operator<<(std::ostream &os, SerialLog::PacketEntry &pe) +{ + +#if 0 + // Dump raw + const uint8_t *p = (const uint8_t *)&pe; + os << "PacketEntry:"; + for (unsigned i = 0; i < sizeof(pe); i++) + os << ' ' << std::setfill('0') << std::setw(2) << std::hex + << unsigned(*p++); +#elif 1 + // Dump formatted + os << "time=" << pe.timestamp << " bytes=" << pe.num_bytes << " flags=" << std::setfill('0') + << std::setw(2) << std::hex << unsigned(uint8_t(pe.flags)); +#endif + + os << std::setfill(' ') << std::setw(0) << std::dec; + + return os; + +} // operator<< + +std::ostream &operator<<(std::ostream &os, SerialLog &sl) +{ + + os << "size=" << sl.size() << " used=" << sl.used() << " free=" << sl.free() + << " enabled=" << (sl.enabled() ? "true" : "false") << std::endl; + +#if 0 + + // Dump raw + + static const unsigned pkt_buf_size = 16; + char pkt_buf[pkt_buf_size]; + + while (sl.used() > 0) + { + + unsigned n = pkt_buf_size; + if (n > sl.used()) + n = sl.used(); + + sl.get(pkt_buf, n); + + for (unsigned i = 0; i < n; i++) + os << ' ' << std::setfill('0') << std::setw(2) << std::hex + << unsigned(uint8_t(pkt_buf[i])); + os << std::endl; + os << std::dec; + + } // while (sl.used()...) + +#elif 1 + + // Dump formatted + + static const unsigned pkt_buf_size = 256; + uint8_t pkt_buf[pkt_buf_size]; + char time_buf[32]; + + while (sl.used() >= sizeof(SerialLog::PacketEntry)) { + SerialLog::PacketEntry pe; + if (!sl.get(&pe)) + return os; + + unsigned keep = pe.num_bytes; + unsigned discard = 0; + if (keep > pkt_buf_size) { + keep = pkt_buf_size; + discard = pe.num_bytes - pkt_buf_size; + } + if (!sl.get(pkt_buf, keep)) + return os; + if (!sl.discard(discard)) + return os; + clock_tostr_r(pe.timestamp, time_buf); + const char *dir = + ((pe.flags & SerialLog::PKTFLG_DIR) == SerialLog::PKTFLG_UP) ? "UP" : "DN"; + os << time_buf << ' ' << std::setw(3) << pe.num_bytes << ' ' << dir << ':'; + for (unsigned i = 0; i < keep; i++) + os << ' ' << std::setfill('0') << std::setw(2) << std::hex << unsigned(pkt_buf[i]); + if (discard == 0) + os << std::endl; + else + os << " ..." << std::endl; // show that we did not print it all + os << std::setfill(' ') << std::dec; + + } // while (sl.used()...) + +#endif + + return os; + +} // operator<< diff --git a/flightcode/stm32/SerialLog.h b/flightcode/stm32/SerialLog.h new file mode 100644 index 0000000..dcb275f --- /dev/null +++ b/flightcode/stm32/SerialLog.h @@ -0,0 +1,114 @@ +#ifndef SERIAL_LOG_H +#define SERIAL_LOG_H + +#include +#include +#include "CircularBuffer.h" + +// Log all traffic to/from the STM32. +// The log is just a circular buffer to which packets are written, +// prepended by a timestamp, length, and flags: +// +// uint64_t timestamp; usec since unix epoch +// uint16_t num_bytes; size of packet[] +// uint8_t flags; upstream or downstream +// uint8_t packet[]; packet bytes, as passed to or received from the slip +// encoder + +class SerialLog +{ + +public: + // Header logged for each packet. + struct __attribute((__packed__)) PacketEntry { + uint64_t timestamp; + uint16_t num_bytes; + uint8_t flags; + uint8_t packet[0]; // variable-length + }; + + SerialLog(unsigned log_size, unsigned timeout_us = 0); + + ~SerialLog(); + + // PacketEntry.flags field + static const int PKTFLG_DIR = 0x01; // mask + static const int PKTFLG_UP = 0x00; // STM32->i.MX6 + static const int PKTFLG_DOWN = 0x01; // i.MX6->STM32 + + // Log a packet (PacketEntry followed by the supplied bytes) + bool log_packet(const void *packet, unsigned num_bytes, uint8_t flags); + + // Return enable status of this logger. Logging is automatically + // disabled when the timeout is reached. + bool enabled(void) const + { + return _enabled; + } + + // Size of logging buffer + unsigned size(void) const + { + return _c_buf.size(); + } + + // Bytes used in buffer + unsigned used(void) const + { + return _c_buf.used(); + } + + // Free bytes in buffer + unsigned free(void) const + { + return _c_buf.free(); + } + + // Get bytes from buffer. + bool get(void *p, unsigned num_bytes) + { + return _c_buf.get(p, num_bytes); + } + + // Get structure from buffer. + template < class type > + bool get(type *p) + { + return get(p, sizeof(type)); + } + + // Discard a number of bytes from the buffer. + bool discard(unsigned num_bytes) + { + return _c_buf.discard(num_bytes); + } + +private: + CircularBuffer _c_buf; + pthread_mutex_t _mutex; + bool _mutex_initted; + unsigned _timeout_us; + uint64_t _last_up_time_us; + bool _enabled; + + // Discard a packet + bool discard_packet(void); + void check_timeout(uint64_t now); + + void lock(void) + { + pthread_mutex_lock(&_mutex); + } + + void unlock(void) + { + pthread_mutex_unlock(&_mutex); + } +}; + +std::ostream &operator<<(std::ostream &, SerialLog::PacketEntry &); + +// destructive dump; log is empty after this +std::ostream &operator<<(std::ostream &, SerialLog &); + +#endif // SERIAL_LOG_H diff --git a/flightcode/stm32/SerialLogTest.cpp b/flightcode/stm32/SerialLogTest.cpp new file mode 100644 index 0000000..c934654 --- /dev/null +++ b/flightcode/stm32/SerialLogTest.cpp @@ -0,0 +1,45 @@ + +#include +#include +#include "SerialLog.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + static const unsigned log_size = 100; + SerialLog sl(log_size); + const char *s; + bool b; + static const unsigned num_pkts = 1000; + unsigned n[num_pkts]; + unsigned pkt_num_oldest = 0; + unsigned total = 0; + unsigned pkt_num; + const char *pkts[10] = {"", "1", "12", "123", "1234", + "12345", "123456", "1234567", "12345678", "123456789"}; + uint8_t f; + const uint8_t flags[2] = {SerialLog::PKTFLG_UP, SerialLog::PKTFLG_DOWN}; + + cout << sl << endl; + + for (pkt_num = 0; pkt_num < num_pkts; pkt_num++) { + s = pkts[pkt_num % 10]; + f = flags[pkt_num % 2]; + cout << "log_packet \"" << s << "\"..."; + b = sl.log_packet(s, strlen(s), f); + n[pkt_num] = sizeof(SerialLog::PacketEntry) + strlen(s); + total += n[pkt_num]; + while (total >= log_size) + total -= n[pkt_num_oldest++]; + if (b && sl.used() == total && sl.free() == sl.size() - total - 1) + cout << "OK" << endl; + else + cout << "ERROR" << endl; + } + + cout << sl << endl; + + return 0; + +} // main diff --git a/flightcode/stm32/SetShotInfo.cpp b/flightcode/stm32/SetShotInfo.cpp new file mode 100644 index 0000000..208f2fd --- /dev/null +++ b/flightcode/stm32/SetShotInfo.cpp @@ -0,0 +1,22 @@ + +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "SetShotInfo.h" + +// Output SetShotInfoMsg to a stream in a human-readable format +ostream &operator<<(ostream &os, const struct SetShotInfoMsg &msg) +{ + os << "descriptor=\"" << msg.descriptor << '\"'; // assume EOS-terminated + return os; +} + +// UDP port on local machine where downstream SetShotInfo should be sent +unsigned SetShotInfo::udpPort = 0; + +SetShotInfo::SetShotInfo(int port) : PacketHandler("127.0.0.1", port, 0x02, PKT_ID_SET_SHOT_INFO) +{ + + SetShotInfo::udpPort = port; + +} // SetShotInfo::SetShotInfo diff --git a/flightcode/stm32/SetShotInfo.h b/flightcode/stm32/SetShotInfo.h new file mode 100644 index 0000000..e5529e8 --- /dev/null +++ b/flightcode/stm32/SetShotInfo.h @@ -0,0 +1,36 @@ +#ifndef SET_SHOT_INFO_H +#define SET_SHOT_INFO_H + +#include +#include +#include "PacketHandler.h" + +// Message as sent to the STM32; message from App is in SoloMessage.h +struct __attribute((__packed__)) SetShotInfoMsg { + // packet ID is prepended just before slip-encoding + char descriptor[0]; + static const int descriptor_max = 64; // (does not appear in struct) +}; + +static const int SetShotInfoMsg_BufSize = sizeof(SetShotInfoMsg) + SetShotInfoMsg::descriptor_max; + +ostream &operator<<(ostream &os, const struct SetShotInfoMsg &msg); + +/*********************************************************************** +Class: The SetShotInfo class. + +Description: Creates a UDP socket for SetShotInfo data. +***********************************************************************/ +class SetShotInfo : public PacketHandler +{ +public: + SetShotInfo(int port); + + // This is nasty, but I need a way to get the UDP port number to which + // downstream messages should be sent from somewhere that does not have + // access to the stm32's setShotInfo object. This is set by the + // constructor. + static unsigned udpPort; +}; + +#endif // SET_SHOT_INFO_H diff --git a/flightcode/stm32/SetTelemUnits.cpp b/flightcode/stm32/SetTelemUnits.cpp new file mode 100644 index 0000000..64135a7 --- /dev/null +++ b/flightcode/stm32/SetTelemUnits.cpp @@ -0,0 +1,21 @@ + +#include "packetTypes.h" +#include "SetTelemUnits.h" + +// UDP port on local machine where downstream SetTelemUnits should be sent +unsigned SetTelemUnits::udpPort = 0; + +SetTelemUnits::SetTelemUnits(int port) : PacketHandler("", port, 0x02, PKT_ID_SET_TELEM_UNITS) +{ + SetTelemUnits::udpPort = port; +} + +void SetTelemUnits::set(string &setting) +{ + char buf = 1; // default is metric + + if (setting == "imperial") + buf = 0; + + sendto(_sock_fd, &buf, 1, 0, (struct sockaddr *)&_sock, sizeof(_sock)); +} diff --git a/flightcode/stm32/SetTelemUnits.h b/flightcode/stm32/SetTelemUnits.h new file mode 100644 index 0000000..d2f5f3d --- /dev/null +++ b/flightcode/stm32/SetTelemUnits.h @@ -0,0 +1,16 @@ +#ifndef SET_TELEM_UNITS_H +#define SET_TELEM_UNITS_H + +#include "PacketHandler.h" + +class SetTelemUnits : public PacketHandler +{ +public: + SetTelemUnits(int port); + + void set(string &setting); + + static unsigned udpPort; +}; + +#endif // SET_TELEM_UNITS_H diff --git a/flightcode/stm32/SoloMessage.cpp b/flightcode/stm32/SoloMessage.cpp new file mode 100644 index 0000000..3477814 --- /dev/null +++ b/flightcode/stm32/SoloMessage.cpp @@ -0,0 +1,51 @@ + +#include +#include +#include "SoloMessage.h" + +using namespace std; + +// instantiate one of these to see debug output at startup +// SoloMessage::Tester tester; + +SoloMessage::Tester::Tester(void) +{ + cout << "SoloMessage::Hdr: " << sizeof(Hdr) << " bytes" << endl; + cout << "SoloMessage::SetButtonString: " << sizeof(SetButtonString) << " bytes" << endl; + cout << "SoloMessage::SetShotString: " << sizeof(SetShotString) << " bytes" << endl; +} + +// Print SoloMessage::Hdr in a human-readable format +ostream &operator<<(ostream &os, const struct SoloMessage::Hdr &msg) +{ + os << "type=" << msg.type; + os << " length=" << msg.length; + const uint8_t *p = (const uint8_t *)&msg + sizeof(msg); + os << setfill('0') << hex; + for (unsigned i = 0; i < msg.length; i++) + os << ' ' << setw(2) << int(p[i]); + os << setfill(' ') << dec; + return os; +} + +// Print SoloMessage::SetButtonString in a human-readable format +ostream &operator<<(ostream &os, const struct SoloMessage::SetButtonString &msg) +{ + os << "type=" << msg.type; + os << " length=" << msg.length; + os << " button_id=" << msg.button_id; + os << " button_event=" << msg.button_event; + os << " shot_id=" << msg.shot_id; + os << " state=" << msg.state; + os << " descriptor=\"" << msg.descriptor << '\"'; // assume EOS-terminated + return os; +} + +// Print SoloMessage::SetShotString in a human-readable format +ostream &operator<<(ostream &os, const struct SoloMessage::SetShotString &msg) +{ + os << "type=" << msg.type; + os << " length=" << msg.length; + os << " descriptor=\"" << msg.descriptor << '\"'; // assume EOS-terminated + return os; +} diff --git a/flightcode/stm32/SoloMessage.h b/flightcode/stm32/SoloMessage.h new file mode 100644 index 0000000..109001a --- /dev/null +++ b/flightcode/stm32/SoloMessage.h @@ -0,0 +1,54 @@ +#ifndef SOLO_MESSAGE_H +#define SOLO_MESSAGE_H + +#include +#include + +// Messages to/from App or ShotMgr + +namespace SoloMessage +{ + +struct Hdr { + uint32_t type; + uint32_t length; // bytes following this header + + // type field: + // MUST MATCH: SoloLink/app/shots/app_packet.py + // MUST MATCH: iSolo/networking/SoloPacket.swift + static const uint32_t GET_CURRENT_SHOT = 0; + static const uint32_t SET_CURRENT_SHOT = 1; + static const uint32_t LOCATION = 2; + static const uint32_t RECORD_POSITION = 3; + static const uint32_t CABLE_CAM_OPTIONS = 4; + // MUST MATCH: SoloLink/flightcode/stm32/btn_msg.py + static const uint32_t BUTTON_EVENT = 2000; + static const uint32_t SET_BUTTON_STRING = 2001; + static const uint32_t SET_SHOT_STRING = 2002; +}; + +struct SetButtonString : public Hdr { + uint8_t button_id; + uint8_t button_event; + uint8_t shot_id; + uint8_t state; + char descriptor[0]; +}; + +struct SetShotString : public Hdr { + char descriptor[0]; +}; + +struct Tester { + Tester(void); +}; + +}; // namespace SoloMessage + +std::ostream &operator<<(std::ostream &os, const struct SoloMessage::Hdr &msg); + +std::ostream &operator<<(std::ostream &os, const struct SoloMessage::SetButtonString &msg); + +std::ostream &operator<<(std::ostream &os, const struct SoloMessage::SetShotString &msg); + +#endif // SOLO_MESSAGE_H diff --git a/flightcode/stm32/SysInfo.cpp b/flightcode/stm32/SysInfo.cpp new file mode 100644 index 0000000..7b63910 --- /dev/null +++ b/flightcode/stm32/SysInfo.cpp @@ -0,0 +1,76 @@ +#include +#include +#include +#include +#include +#include +#include "SysInfo.h" +#include "packetTypes.h" + +using namespace std; + +/* +The packet ID has already been stripped from the front. The packet as +received here looks like this (names are from artoo source): + + --DATA-------------- --LENGTH-------------------- --AS OF 9/25/14--- + Sys::UniqueId Sys::UniqueIdLen 12 bytes + Sys::HardwareVersion sizeof(Sys::HardwareVersion) 2 bytes + Version::str() strlen(Version::str()) variable + -------------------- ---------------------------- ------------------ + +The UniqueId and HardwareVersion are (9/25/14) compiled-in, and the +version string is generated at build time from `git describe --tags`. +*/ + +static unsigned pingCount = 0; + +int SysInfo::upHandler(char serBuf[], int len) +{ + const unsigned uniqueIdLen = 12; // match artoo/src/stm32/sys.h + uint8_t uniqueId[uniqueIdLen]; + uint16_t hardwareVersion; // match artoo/src/hostprotocol.cpp, hwversion + char version[64]; // typically around 8 chars + static bool infoLogged = false; + + if (len < 14) + syslog(LOG_ERR, "sys: message too short (%d)", len); + + memcpy(uniqueId, &serBuf[0], sizeof(uniqueId)); + + memcpy(&hardwareVersion, &serBuf[12], sizeof(hardwareVersion)); + + // make sure version is \0 terminated + // variable length, from 14 to end of message + unsigned copyLen = len - 14; + if (copyLen > (sizeof(version) - 1)) + copyLen = sizeof(version) - 1; + memset(version, 0, sizeof(version)); + memcpy(version, &serBuf[14], copyLen); + + if (!infoLogged) { + char uid[40]; // need 36 + snprintf(uid, sizeof(uid), "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", + uniqueId[0], uniqueId[1], uniqueId[2], uniqueId[3], uniqueId[4], uniqueId[5], + uniqueId[6], uniqueId[7], uniqueId[8], uniqueId[9], uniqueId[10], uniqueId[11]); + syslog(LOG_INFO, "sys: unique id: %s", uid); + syslog(LOG_INFO, "sys: hardware ver: %u", hardwareVersion); + syslog(LOG_INFO, "sys: software ver: %s", version); + syslog(LOG_INFO, "sys: (try %u)", pingCount); + infoLogged = true; + } + + return 0; // we don't send anything +} + +// Sends a blank byte to the STM32 as a "ping" +// Note that this send is using a socket to send to itself; seems odd, but the +// point is to enqueue the message for the downstream thread to send over +// serial, i.e. the send is generally from a different thread than the receive +// will be. +void SysInfo::ping(void) +{ + char buf = ' '; + sendto(_sock_fd, &buf, 1, 0, (struct sockaddr *)&_sock, sizeof(_sock)); + pingCount++; +} diff --git a/flightcode/stm32/SysInfo.h b/flightcode/stm32/SysInfo.h new file mode 100644 index 0000000..32d1694 --- /dev/null +++ b/flightcode/stm32/SysInfo.h @@ -0,0 +1,30 @@ +#ifndef _SYSINFO_H_ +#define _SYSINFO_H_ + +#include +#include "PacketHandler.h" +#include "packetTypes.h" +#include "net_wmm.h" + +using namespace std; + +/*********************************************************************** +Class: The sysinfo class. + +Description: Creates a UDP socket for system info. +***********************************************************************/ + +class SysInfo : public PacketHandler +{ +public: + // Constructor. Takes an IP address and port + SysInfo(string ipaddr, int port) : PacketHandler(ipaddr, port, 0x03, PKT_ID_SYS_INFO){}; + + // Overriden upHandler to read STM32 board data + int upHandler(char serBuf[], int len); + + // Send a blank packet to the sysinfo port to "ping" the STM32 + void ping(void); +}; + +#endif //_SYSINFO_H diff --git a/flightcode/stm32/TcpServer.cpp b/flightcode/stm32/TcpServer.cpp new file mode 100644 index 0000000..6ea20f2 --- /dev/null +++ b/flightcode/stm32/TcpServer.cpp @@ -0,0 +1,642 @@ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "mutex.h" +#include "ButtonFunctionCfg.h" +#include "SetShotInfo.h" +#include "TcpServer.h" + +//#define NDEBUG +#include + +using namespace std; + +TcpClient::TcpClient(TcpServer *server, int fd_client) + : _server(server), _fd_client(fd_client), _buf_bytes(0) +{ + int sockopt = 1; + if (setsockopt(fd_client, IPPROTO_TCP, TCP_NODELAY, &sockopt, sizeof(sockopt)) != 0) + syslog(LOG_ERR, "TcpClient: setsockopt TCP_NODELAY: %s", strerror(errno)); +} + +TcpClient::~TcpClient() +{ + close(_fd_client); + _fd_client = -1; + _server = NULL; +} + +// Stuff that is always true +void TcpClient::invariant(void) const +{ + + assert(_server != NULL); + assert(_fd_client > 0); + assert(_buf_bytes <= _buf_len); + +} // TcpClient::invariant + +int TcpClient::send_upstream(const void *data, int data_len) +{ + invariant(); + // MSG_NOSIGNAL means if the remote end closes the connection, we + // don't want the SIGPIPE; just return from send with errno EPIPE + // MSG_DONTWAIT means if the send would block, return EAGAIN or + // EWOULDBLOCK instead. We will drop the message, but in our cases + // (low rate small messages) this means there is something wrong + // with the connection, and it will likely be closed anyway. The + // absolute worst case is we lose a button event, which should not + // cause any harm (as if the button were never pressed). + return send(_fd_client, data, data_len, MSG_NOSIGNAL | MSG_DONTWAIT); +} + +// Receive from fd, appending data to buffer. +// Check for complete message in buffer and send downstream if so. +// Return true if data received, false if client is gone. +bool TcpClient::do_recv(void) +{ + bool status; + int res; + + invariant(); + + res = recv(_fd_client, _buf + _buf_bytes, _buf_len - _buf_bytes, 0); + // cout << "TcpClient:do_recv: recv returned " << res << endl; + if (res > 0) { + // new data appended to _buf + _buf_bytes += res; + + // don't look at message header until we have enough for it + while (_buf_bytes >= sizeof(SoloMessage::Hdr)) { + SoloMessage::Hdr *hdr = (SoloMessage::Hdr *)_buf; + + // message is SoloMessage::Hdr plus payload + unsigned msg_len = sizeof(SoloMessage::Hdr) + hdr->length; + + if (_buf_bytes < msg_len) + break; + + // have a complete message; format and send downstream + + // cout << "TcpClient: have downstream message:" << endl; + // cout << *hdr << endl; + + if (hdr->type == SoloMessage::Hdr::SET_BUTTON_STRING) { + SoloMessage::SetButtonString *appMsg; + + appMsg = (SoloMessage::SetButtonString *)_buf; + + // struct ButtonFunctionCfgMsg does not include the descriptor + // plant the message on top of a buffer big enough + char stm32Buf[ButtonFunctionCfgMsg_BufSize]; + memset(stm32Buf, 0, sizeof(stm32Buf)); + ButtonFunctionCfgMsg *stm32Msg = (ButtonFunctionCfgMsg *)stm32Buf; + + stm32Msg->button_id = appMsg->button_id; + stm32Msg->button_event = appMsg->button_event; + stm32Msg->shot_id = appMsg->shot_id; + stm32Msg->state = appMsg->state; + // descriptor is everything after pad to the end + int msgBodyLen = sizeof(SoloMessage::SetButtonString) - sizeof(SoloMessage::Hdr); + int descLen = appMsg->length - msgBodyLen; + // if too long, chop it off + if (descLen > (ButtonFunctionCfgMsg::descriptor_max - 1)) + descLen = (ButtonFunctionCfgMsg::descriptor_max - 1); + memcpy(stm32Msg->descriptor, appMsg->descriptor, descLen); + + // cout << "TcpClient: sending downstream:" << endl; + // cout << *stm32Msg << endl; + + // Send it to the downstream UDP port for this message. + // (All downstream messages have to be sent via the stm32 + // downstream thread to avoid contention on the serial + // interface.) + struct sockaddr_in sa; + memset((char *)&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + assert(ButtonFunctionCfg::udpPort != 0); + sa.sin_port = htons(ButtonFunctionCfg::udpPort); + sendto(_server->stm32_fd(), stm32Msg, sizeof(*stm32Msg) + descLen, 0, + (struct sockaddr *)&sa, sizeof(sa)); + } else if (hdr->type == SoloMessage::Hdr::SET_SHOT_STRING) { + SoloMessage::SetShotString *appMsg; + + appMsg = (SoloMessage::SetShotString *)_buf; + + // struct SetShotInfoMsg does not include the descriptor + // plant the message on top of a buffer big enough + char stm32Buf[SetShotInfoMsg_BufSize]; + memset(stm32Buf, 0, sizeof(stm32Buf)); + SetShotInfoMsg *stm32Msg = (SetShotInfoMsg *)stm32Buf; + + // descriptor is everything to the end + int msgBodyLen = sizeof(SoloMessage::SetShotString) - sizeof(SoloMessage::Hdr); + int descLen = appMsg->length - msgBodyLen; + // if too long, chop it off + if (descLen > (SetShotInfoMsg::descriptor_max - 1)) + descLen = (SetShotInfoMsg::descriptor_max - 1); + memcpy(stm32Msg->descriptor, appMsg->descriptor, descLen); + + // cout << "TcpClient: sending downstream:" << endl; + // cout << *stm32Msg << endl; + + // Send it to the downstream UDP port for this message. + // (All downstream messages have to be sent via the stm32 + // downstream thread to avoid contention on the serial + // interface.) + struct sockaddr_in sa; + memset((char *)&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + assert(SetShotInfo::udpPort != 0); + sa.sin_port = htons(SetShotInfo::udpPort); + sendto(_server->stm32_fd(), stm32Msg, sizeof(*stm32Msg) + descLen, 0, + (struct sockaddr *)&sa, sizeof(sa)); + } + + // done with the message at the start of the buffer; + // copy any unused data after it back to the start + _buf_bytes -= msg_len; // how much is left over + memmove(_buf, _buf + msg_len, _buf_bytes); + + } // while (_buf_bytes...) + + status = true; + + } else // res <= 0 + { + status = false; + } + + invariant(); + + return status; + +} // TcpClient::do_recv + +// Constructor initializes object; call start method to open socket and start +// threads. +TcpServer::TcpServer(int port, const char *name) + : _name(name), _listen_port(port), _listen_fd(-1), _stm32_fd(-1), + // _clients + // _clients_mutex + _listen_sfd(-1), _downstream_sfd(-1) +// _listen_id +// _downstream_id +{ +} + +TcpServer::~TcpServer() +{ +} + +int TcpServer::start(void) +{ + int sockopt; + struct sockaddr_in sa; + sigset_t mask; + int res; + + // initialize clients list mutex + res = mutex_init(&_clients_mutex); + if (res != 0) { + syslog(LOG_ERR, "mutex_init: %d", res); + goto start_exit_0; + } + + // create listening socket + _listen_fd = socket(AF_INET, SOCK_STREAM, 0); + if (_listen_fd < 0) { + syslog(LOG_ERR, "socket: %s", strerror(errno)); + goto start_exit_1; + } + + // non-blocking - this handles the race where a client tries to connect, + // the select() activates, then the client is gone before the accept. + if (fcntl(_listen_fd, F_SETFL, O_NONBLOCK) != 0) { + syslog(LOG_ERR, "fcntl: %s", strerror(errno)); + goto start_exit_2; + } + + // allow re-binding to our port in case of restart + sockopt = 1; + if (setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(sockopt)) != 0) { + syslog(LOG_ERR, "setsockopt: %s", strerror(errno)); + goto start_exit_2; + } + + // bind to port where we listen for connections + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(_listen_port); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + if (bind(_listen_fd, (struct sockaddr *)&sa, sizeof(sa)) == -1) { + syslog(LOG_ERR, "bind: %s", strerror(errno)); + goto start_exit_2; + } + + // listen for connections + if (listen(_listen_fd, 10) == -1) { + syslog(LOG_ERR, "listen: %s", strerror(errno)); + goto start_exit_2; + } + + res = sigemptyset(&mask); + assert(res == 0); // invalid arguments + res = sigaddset(&mask, SIGHUP); + assert(res == 0); // invalid arguments + res = sigaddset(&mask, SIGQUIT); + assert(res == 0); // invalid arguments + res = pthread_sigmask(SIG_BLOCK, &mask, NULL); + assert(res == 0); // invalid arguments + + // create listen signal fd + res = sigemptyset(&mask); + assert(res == 0); // invalid arguments + res = sigaddset(&mask, SIGQUIT); + assert(res == 0); // invalid arguments + _listen_sfd = signalfd(-1, &mask, 0); + if (_listen_sfd < 0) { + syslog(LOG_ERR, "signalfd: %s", strerror(errno)); + goto start_exit_2; + } + + // create downstream thread signal fd + res = sigemptyset(&mask); + assert(res == 0); // invalid arguments + res = sigaddset(&mask, SIGHUP); + assert(res == 0); // invalid arguments + res = sigaddset(&mask, SIGQUIT); + assert(res == 0); // invalid arguments + _downstream_sfd = signalfd(-1, &mask, 0); + if (_downstream_sfd < 0) { + syslog(LOG_ERR, "signalfd: %s", strerror(errno)); + goto start_exit_3; + } + + // UDP socket that will be used by all clients to send messages to an + // stm32 downstream port + _stm32_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (_stm32_fd < 0) { + syslog(LOG_ERR, "socket: %s", strerror(errno)); + goto start_exit_4; + } + + // bind to any IP, any port */ + memset((char *)&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(0); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + if (bind(_stm32_fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + syslog(LOG_ERR, "bind: %s", strerror(errno)); + goto start_exit_5; + } + + // start listen_thread + if (pthread_create(&_listen_id, NULL, &TcpServer::listen_entry, this) != 0) { + syslog(LOG_ERR, "pthread_create: %s", strerror(errno)); + goto start_exit_5; + } + + if (pthread_setname_np(_listen_id, "stm32_listen") != 0) { + syslog(LOG_ERR, "pthread_setname_np: %s", strerror(errno)); + goto start_exit_6; + } + + // start downstream_thread + if (pthread_create(&_downstream_id, NULL, &TcpServer::downstream_entry, this) != 0) { + syslog(LOG_ERR, "pthread_create: %s", strerror(errno)); + goto start_exit_6; + } + + if (pthread_setname_np(_downstream_id, "stm32_recv") != 0) { + syslog(LOG_ERR, "pthread_setname_np: %s", strerror(errno)); + goto start_exit_7; + } + + return 0; + +// WARNING: Error conditions are not tested. +// Errors here are probably fatal to the system. + +start_exit_7: + thread_kill(_downstream_id); + thread_wait(_downstream_id); + +start_exit_6: + thread_kill(_listen_id); + thread_wait(_listen_id); + +start_exit_5: + close(_stm32_fd); + _stm32_fd = -1; + +start_exit_4: + close(_downstream_sfd); + _downstream_sfd = -1; + +start_exit_3: + close(_listen_sfd); + _listen_sfd = -1; + +start_exit_2: + close(_listen_fd); + _listen_fd = -1; + +start_exit_1: + (void)pthread_mutex_destroy(&_clients_mutex); + +start_exit_0: + return -1; + +} // TcpServer::start + +// WARNING: Stopping is not tested. Normal operation is to start and run +// forever, or stop by killing the entire process. +void TcpServer::stop(void) +{ + + thread_kill(_downstream_id); + thread_kill(_listen_id); + + thread_wait(_downstream_id); + thread_wait(_listen_id); + + close(_stm32_fd); + _stm32_fd = -1; + + close(_downstream_sfd); + _downstream_sfd = -1; + + close(_listen_sfd); + _listen_sfd = -1; + + close(_listen_fd); + _listen_fd = -1; + + (void)pthread_mutex_destroy(&_clients_mutex); + +} // TcpServer::stop + +// Tell a thread to exit. We send it SIGQUIT; it is expected to be waiting +// on a signalfd where this will arrive, and know that SIGQUIT means quit. +void TcpServer::thread_kill(pthread_t id) +{ + // SIGQUIT means quit + if (pthread_kill(id, SIGQUIT) != 0) + syslog(LOG_ERR, "pthread_kill: %s", strerror(errno)); +} + +// Wait for a thread to exit. It should have already received the SIGQUIT, so +// should soon get it and cleanly exit. If it does not, we attempt to kill it. +void TcpServer::thread_wait(pthread_t id) +{ + // The timeout to pthread_timedjoin_np() is an absolute time, which + // can break if the time is changed while waiting. Instead we poll + // pthread_tryjoin_np() few times. + + // wait up to 10 msec + for (int try_num = 0; try_num < 10; try_num++) { + if (pthread_tryjoin_np(id, NULL) == 0) + return; + usleep(1000); // 1 msec + } + + // the join never succeeded; try canceling + syslog(LOG_ERR, "thread_wait: canceling thread"); + pthread_cancel(id); + // thread cleans up asynchronously + usleep(10000); // 10 msec + +} // TcpServer::thread_wait + +// Listening thread. +// Create a socket and listen for connections. When a connection is received, +// the client is added to the list of clients. The list of clients determines +// what is monitored for incoming data (select, in downstream_thread) and who +// gets all upstream messages (send_clients method). +void *TcpServer::listen_thread(void) +{ + int nfds; + fd_set rfds; + + syslog(LOG_INFO, "%s listen_thread: starting", _name); + + while (true) { + + // Select waits for either an incoming connection on _listen_fd or a + // signal on _listen_sfd. + FD_ZERO(&rfds); + nfds = 0; + + FD_SET(_listen_sfd, &rfds); + if (nfds <= _listen_sfd) + nfds = _listen_sfd + 1; + + FD_SET(_listen_fd, &rfds); + if (nfds <= _listen_fd) + nfds = _listen_fd + 1; + + int res = select(nfds, &rfds, NULL, NULL, NULL); + + if (res > 0) { + + // got a signal? + if (FD_ISSET(_listen_sfd, &rfds)) { + // clear out the signal + signalfd_siginfo si; + if (read(_listen_sfd, &si, sizeof(si)) != sizeof(si)) { + syslog(LOG_ERR, "%s listen_thread: reading siginfo", _name); + break; // while (true) + } else if (si.ssi_signo == SIGQUIT) { + // SIGQUIT means quit + break; // while (true) + } else { + // mystery signal + syslog(LOG_ERR, "%s listen_thread: unexpected signal", _name); + } + } + + // got a connection? + if (FD_ISSET(_listen_fd, &rfds)) { + // Incoming client - this will not block (the socket is + // nonblocking). If the client is no longer trying to + // connect, we get an invalid fd_client (immediately) and + // handle that. + struct sockaddr_in sa; + memset((char *)&sa, 0, sizeof(sa)); + socklen_t sa_len = sizeof(sa); + int fd_client = accept(_listen_fd, (struct sockaddr *)&sa, &sa_len); + if (fd_client < 0) { + // client tried to connect then changed its mind + syslog(LOG_ERR, "accept: %s", strerror(errno)); + usleep(10000); // 10ms + continue; + } + + if (sa.sin_family == AF_INET) + syslog(LOG_INFO, "%s listen_thread: adding client %d at %s:%u", _name, + fd_client, inet_ntoa(sa.sin_addr), ntohs(sa.sin_port)); + else + syslog(LOG_INFO, "%s listen_thread: adding client %d", _name, fd_client); + + // Add new client to the list of clients that gets everything + // via send_clients(). Clients are removed from this list when + // send_clients() determines that the connection is dead. + TcpClient *new_client = new TcpClient(this, fd_client); + pthread_mutex_lock(&_clients_mutex); + _clients.push_back(new_client); + pthread_mutex_unlock(&_clients_mutex); + + // Knock downstream_thread out of its select, so it will + // rebuild the fdset with the new client (we use SIGHUP). + if (pthread_kill(_downstream_id, SIGHUP) != 0) + syslog(LOG_ERR, "pthread_kill: %s", strerror(errno)); + } + } else { + syslog(LOG_ERR, "%s listen_thread: select: %s", _name, strerror(errno)); + } + + } // while (true) + + syslog(LOG_INFO, "%s listen_thread: exiting", _name); + + return NULL; + +} // TcpServer::listen_thread + +// Downstream thread. +// Select on clients, waiting for data to arrive for one. +// Read data, letting each client accumulate data and send a message +// downstream when it has one. +void *TcpServer::downstream_thread(void) +{ + int nfds; + fd_set rfds; + list< TcpClient * >::iterator it, next; + int res; + int fd; + + syslog(LOG_INFO, "%s downstream_thread: starting", _name); + + while (true) { + + // build set of fds to wait on + FD_ZERO(&rfds); + nfds = 0; + + // signal fd is always in the set + fd = _downstream_sfd; + FD_SET(fd, &rfds); + if (nfds <= fd) + nfds = fd + 1; + + // add all the clients to the set + pthread_mutex_lock(&_clients_mutex); + for (it = _clients.begin(); it != _clients.end(); it++) { + fd = (*it)->fd_client(); + FD_SET(fd, &rfds); + if (nfds <= fd) + nfds = fd + 1; + } + // unsigned num_clients = _clients.size(); + pthread_mutex_unlock(&_clients_mutex); + + // wait for something on any of them (or a signal) + res = select(nfds, &rfds, NULL, NULL, NULL); + + if (res > 0) { + + // signal? + if (FD_ISSET(_downstream_sfd, &rfds)) { + signalfd_siginfo si; + if (read(_downstream_sfd, &si, sizeof(si)) != sizeof(si)) { + syslog(LOG_ERR, "%s downstream_thread: reading siginfo", _name); + break; // while (true) + } else if (si.ssi_signo == SIGQUIT) { + // SIGQUIT means quit + break; // while (true) + } else if (si.ssi_signo == SIGHUP) { + // SIGHUP means recreate the fdset we're waiting on + } else { + // mystery signal + syslog(LOG_ERR, "%s downstream_thread: unexpected signal", _name); + } + } + + // data from any client? + pthread_mutex_lock(&_clients_mutex); + for (it = _clients.begin(); it != _clients.end(); it = next) { + next = it; + next++; + TcpClient *c = *it; + if (FD_ISSET(c->fd_client(), &rfds)) { + if (!c->do_recv()) { + syslog(LOG_INFO, "%s downstream_thread: deleting client %d", _name, + c->fd_client()); + _clients.erase(it); + delete c; + } + } + } + pthread_mutex_unlock(&_clients_mutex); + } else // res <= 0 + { + syslog(LOG_ERR, "%s downstream_thread: select returned %d", _name, res); + } + + } // while (true) + + syslog(LOG_INFO, "%s downstream_thread: exiting", _name); + + return NULL; + +} // TcpServer::downstream_thread + +// Send data to all clients. +// The supplied data is sent to all clients in the client list, i.e. all +// clients that have connected to the server. +void TcpServer::send_clients(const void *data, int data_len) +{ + list< TcpClient * >::iterator it; + + pthread_mutex_lock(&_clients_mutex); + + for (it = _clients.begin(); it != _clients.end(); it++) { + uint64_t t0_us = clock_gettime_us(CLOCK_MONOTONIC); + int rc = (*it)->send_upstream(data, data_len); + uint64_t t1_us = clock_gettime_us(CLOCK_MONOTONIC); + if (rc != data_len) { + // The client might be gone; downstream thread detects that + // and deletes it + syslog(LOG_ERR, "%s send_clients: %s", _name, strerror(errno)); + } + unsigned delay_us = t1_us - t0_us; + if (delay_us > 10000) { + // Looking for trouble - when does send_upstream block? + syslog(LOG_ERR, "%s send_clients: send_upstream took %u usec", _name, delay_us); + } + } + + pthread_mutex_unlock(&_clients_mutex); + +} // TcpServer::send_clients diff --git a/flightcode/stm32/TcpServer.h b/flightcode/stm32/TcpServer.h new file mode 100644 index 0000000..cfd7408 --- /dev/null +++ b/flightcode/stm32/TcpServer.h @@ -0,0 +1,108 @@ +#ifndef _TCP_SERVER_H_ +#define _TCP_SERVER_H_ + +#include +#include +#include +#include +#include "SoloMessage.h" + +class TcpServer; + +// One of these for each connection accepted, containing required +// state for the connection. +class TcpClient +{ + +public: + TcpClient(TcpServer *server, int fd_client); + ~TcpClient(); + int send_upstream(const void *data, int data_len); + bool do_recv(void); + inline int fd_client(void) const + { + return _fd_client; + }; + +private: + // Stuff shared by all clients is kept in the server + TcpServer *_server; + + // File descriptor for this client. This is already open when the + // client is constructed, but is closed by the client destructor. + // This is the TCP stream to/from the client. + int _fd_client; + + // Since the connection is TCP, we might get more or less than one + // message on each read. Data is buffered here until we have at least + // one message, then the message is sent downstream, and any leftover + // data is copied to the front of the buffer (since it is part of the + // next message). + const static unsigned _buf_len = 256; + char _buf[_buf_len]; + // How much is currently in _buf. + unsigned _buf_bytes; + + void invariant(void) const; + + friend TcpServer; + +}; // TcpClient + +// One of these per listening port. listen_thread thread listens on a socket +// for client connections, and downstream_thread selects on a set of sockets, +// one for each client, waiting for downstream messages. Upstream messages are +// sent via the send_clients method. +class TcpServer +{ +public: + TcpServer(int port, const char *name = "anonymous"); + ~TcpServer(); + + int start(void); + void stop(void); + + void send_clients(const void *data, int num_bytes); + + inline int stm32_fd(void) + { + return _stm32_fd; + } + +private: + const char *_name; + const int _listen_port; + int _listen_fd; + int _stm32_fd; + + std::list< TcpClient * > _clients; + pthread_mutex_t _clients_mutex; + + // signal fds, used to wake up threads out of select + int _listen_sfd; + int _downstream_sfd; + + void thread_kill(pthread_t id); + void thread_wait(pthread_t id); + + pthread_t _listen_id; + void *listen_thread(void); + // listen_entry is a pthread_create entry point + static void *listen_entry(void *arg) + { + TcpServer *me = (TcpServer *)arg; + return me->listen_thread(); + } + + pthread_t _downstream_id; + void *downstream_thread(void); + // downstream_entry is a pthread_create entry point + static void *downstream_entry(void *arg) + { + TcpServer *me = (TcpServer *)arg; + return me->downstream_thread(); + } + +}; // TcpServer + +#endif // _TCP_SERVER_H_ diff --git a/flightcode/stm32/Telem.cpp b/flightcode/stm32/Telem.cpp new file mode 100644 index 0000000..507cded --- /dev/null +++ b/flightcode/stm32/Telem.cpp @@ -0,0 +1,146 @@ +#include +#include +#include +#include +#include +#include +#include +#include "Telem.h" +#include "SerialLog.h" +#include "packetTypes.h" +#include "net_wmm.h" +#include "../mavlink/c_library/common/mavlink.h" +#include "SLIP.h" +#include "util.h" +#include "link_packet.h" + +Telem::Telem(string ipaddr, int port, unsigned log_gap_us, unsigned log_delay_max, + string &log_delay_filename) + : PacketHandler(ipaddr, port, 0xe0, PKT_ID_MAVLINK), _log_gap_us(log_gap_us), + _log_delay_max(log_delay_max), _log_delay_filename(log_delay_filename), _fp_delay(NULL), + _delay_cnt(0) +{ + pkt_delay_open(); +} + +/*********************************************************************** +Method: Telem data handler + +Description: Transmits telem data from UDP to STM32 via serial +***********************************************************************/ +void Telem::downHandler(int ser_fd, int verbosity) +{ + LinkPacket packet; + socklen_t addrlen = sizeof(_sock); + int recvlen; + char msg[1024]; + int encodedLen; + SLIPEncoder *slipEnc = new SLIPEncoder(msg, sizeof(msg)); + static uint64_t last_stm_recv_us = 0; + + // Attempt to receive data. This should be good since we got here from + // a select() + recvlen = recvfrom(_sock_fd, &packet, sizeof(packet), 0, (struct sockaddr *)&_sock, &addrlen); + + packet.stm_recv_us = clock_gettime_us(CLOCK_MONOTONIC); + + // Log any gaps in telemetry >= _log_gap_us + if (last_stm_recv_us != 0) { + unsigned gap_us = packet.stm_recv_us - last_stm_recv_us; + if (gap_us >= _log_gap_us) + syslog(LOG_INFO, "telem gap %u msec", (gap_us + 500) / 1000); + } + last_stm_recv_us = packet.stm_recv_us; + + pkt_delay_write(packet); + +#ifdef INCLUDE_SERIAL_LOG + extern SerialLog *serialLog; +#endif + + char *b; + uint8_t *p_payload = packet.payload; + while (p_payload < ((uint8_t *)&packet + recvlen)) { + + /* Make sure this is a mavlink packet */ + if (p_payload[0] != 0xFE) { + syslog(LOG_ERR, "pkt: Got a bad mavlink packet in the LinkPacket"); + break; + } + + // Since this should always be mavlink, make sure its not a dataflash log type + // Check index 5 for the message type + if (p_payload[5] == MAVLINK_MSG_ID_LOG_DATA || + p_payload[5] == MAVLINK_MSG_ID_LOG_DATA_LEN || + p_payload[5] == MAVLINK_MSG_ID_LOG_DATA_CRC) + goto move_on; + + /* Use the existing packet, even though we'll lose some data. + * Stick the id type before the start of the mavlink message.*/ + b = (char *)(p_payload - 1); + b[0] = _pktID; + encodedLen = slipEnc->encode(b, (p_payload[1] + 8) + 1); + +#ifdef INCLUDE_SERIAL_LOG + // Note that dataflash downlink is not logged, + // since it is not sent to the STM32 + serialLog->log_packet(b, encodedLen, SerialLog::PKTFLG_DOWN); +#endif // INCLUDE_SERIAL_LOG + + if (write(ser_fd, msg, encodedLen) != encodedLen) + syslog(LOG_ERR, "pkt: error writing to serial port"); + move_on: + p_payload += p_payload[1] + 8; + } +} + +void Telem::pkt_delay_open(void) +{ + + // packet time delay log enabled by setting _log_delay_filename != "" + if (_log_delay_filename != "") { + // If the file exists, this truncates it and starts over ("w", not "a"). + // The edge case to avoid is if the rename below fails for whatever + // reason, this file does not grow forever (eating RAM). + _fp_delay = fopen(_log_delay_filename.c_str(), "w"); + if (_fp_delay != NULL) { + char time_buf[40]; + fprintf(_fp_delay, "\n"); + fprintf(_fp_delay, "starting: %s\n", clock_gettime_str_r(CLOCK_REALTIME, time_buf)); + fprintf(_fp_delay, "seq,read_len,blocked,tf_recv,tf_send,tc_recv,tc_send,stm_recv\n"); + fflush(_fp_delay); + } + } + +} // Telem::pkt_delay_open + +void Telem::pkt_delay_write(LinkPacket &packet) +{ + if (_fp_delay == NULL) + return; + + fprintf(_fp_delay, "%u,%u,%u,%llu,%llu,%llu,%llu,%llu\n", packet.seq, packet.data1, + packet.data2, packet.tf_recv_us, packet.tf_send_us, packet.tc_recv_us, + packet.tc_send_us, packet.stm_recv_us); + + _delay_cnt++; + if (_delay_cnt < _log_delay_max) { + fflush(_fp_delay); + return; + } + + // move file.csv to file.csv.1 (losing the old .1 if it exists), + // and open a new file.csv + + fclose(_fp_delay); + _fp_delay = NULL; + _delay_cnt = 0; + + string fn_new = _log_delay_filename + ".1"; + + // if the rename fails, we lose the .1 file and start over with the new one + (void)rename(_log_delay_filename.c_str(), fn_new.c_str()); + + pkt_delay_open(); + +} // Telem::pkt_delay_write diff --git a/flightcode/stm32/Telem.h b/flightcode/stm32/Telem.h new file mode 100644 index 0000000..70d8bb5 --- /dev/null +++ b/flightcode/stm32/Telem.h @@ -0,0 +1,36 @@ +#ifndef _TELEM_H_ +#define _TELEM_H_ + +#include +#include +#include "link_packet.h" +#include "PacketHandler.h" + +/*********************************************************************** +Class: The telem class. + +Description: Creates a UDP socket for telemetry data, which is sent + bidirectionally to and from the stm32. +***********************************************************************/ + +class Telem : public PacketHandler +{ +public: + // Constructor. Takes an IP address and port + Telem(string ipaddr, int port, unsigned log_gaps_us, unsigned pkt_delay_max, + string &pkt_delay_filename); + + // Special downhandler that checks mavlink types + void downHandler(int ser_fd, int verbosity = 0); + +private: + void pkt_delay_open(void); + void pkt_delay_write(LinkPacket &packet); + unsigned _log_gap_us; + unsigned _log_delay_max; + string _log_delay_filename; + FILE *_fp_delay; + unsigned _delay_cnt; +}; + +#endif //_TELEM_H diff --git a/flightcode/stm32/Updater.h b/flightcode/stm32/Updater.h new file mode 100644 index 0000000..06755ed --- /dev/null +++ b/flightcode/stm32/Updater.h @@ -0,0 +1,26 @@ +#ifndef UPDATER_H +#define UPDATER_H + +#include +#include +#include +#include +#include "packetTypes.h" +#include "net_wmm.h" +#include "PacketHandler.h" + +using namespace std; + +/*********************************************************************** +Class: The Updater class. + +Description: Creates a UDP socket for updater message data. +***********************************************************************/ +class Updater : public PacketHandler +{ +public: + // Constructor. Takes an IP address, port and TOS value + Updater(string ipaddr, int port) : PacketHandler(ipaddr, port, 0x02, PKT_ID_UPDATER){}; +}; + +#endif // UPDATER_H diff --git a/flightcode/stm32/app_connected_msg.py b/flightcode/stm32/app_connected_msg.py new file mode 100755 index 0000000..8caeadf --- /dev/null +++ b/flightcode/stm32/app_connected_msg.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +# Send "app connected" message +# Required artoo 0.7.9 or later. + +import socket +import struct +import sys + +DISCONNECTED = 0 +CONNECTED = 1 + +# Must match flightcode/stm32 +APP_CONNECTED_PORT = 5026 + +def send(app_connected): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + msg = struct.pack("!B", app_connected) + s.sendto(msg, ("127.0.0.1", APP_CONNECTED_PORT)) + s.close() + +def send_connected(): + send(CONNECTED) + +def send_disconnected(): + send(DISCONNECTED) + +def usage(): + print "usage: app_connected.py " + print " where is one of" + print " c[onnected] tell stm32 app is connected" + print " d[isconnected] tell stm32 app is disconnected" + +if __name__ == "__main__": + # one required argument: "c[onnected]" or "d[isconnected]" + if len(sys.argv) != 2: + usage() + elif sys.argv[1][0] == "c": + send_connected() + elif sys.argv[1][0] == "d": + send_disconnected() + else: + usage() diff --git a/flightcode/stm32/btn_client.py b/flightcode/stm32/btn_client.py new file mode 100755 index 0000000..30d50db --- /dev/null +++ b/flightcode/stm32/btn_client.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +import socket +import struct +import threading +from sololink import btn_msg + +HOST = "10.1.1.1" +PORT = 5016 + +s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +print "connecting to", HOST, ":", PORT, "...", +s.connect((HOST, PORT)) +print "OK" + +def in_thread(s): + while True: + msg = btn_msg.recv(s) + if msg is None: + print "received \"None\"" + break + elif len(msg) != 4: + print "received funky message: ", str(msg) + else: + print "received:", msg[0], \ + btn_msg.ButtonName[msg[1]], \ + btn_msg.EventName[msg[2]], msg[3] + if btn_msg.msg_buf_long != 0: + print "btn_msg.msg_buf_long=%d!" % btn_msg.msg_buf_long + btn_msg.msg_buf_long = 0 + if btn_msg.msg_buf_short != 0: + print "btn_msg.msg_buf_short=%d!" % btn_msg.msg_buf_short + btn_msg.msg_buf_short = 0 + +in_id = threading.Thread(target=in_thread, args=(s,)) +in_id.daemon = True +in_id.start() + +format = 1 +while True: + + desc = raw_input() + + # allow changing which message goes out + if desc == "1": + format = 1 + continue; + elif desc == "2": + format = 2 + continue; + + if format == 1: + button_id = btn_msg.ButtonA + shot_id = 0 + btn_msg.sendArtooString(s, button_id, shot_id, desc + "\0") + elif format == 2: + btn_msg.sendShotString(s, desc + "\0") diff --git a/flightcode/stm32/config_stick_axes.py b/flightcode/stm32/config_stick_axes.py new file mode 100755 index 0000000..8371d89 --- /dev/null +++ b/flightcode/stm32/config_stick_axes.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python + +import pprint +import ConfigParser +import sys +import config_stick_axes_msg +import param_stored_vals_msg + + +def usage(): + print "usage: config_stick_axes.py set " + print " set stick mapping using config file" + print " config_stick_axes.py get " + print " get stick mapping and compare to config file (\"true\", \"false\")" + + +# read one field from config file and return as integer +def get_field(cfg, axis, field, valid_range=None): + # if not specified, leave uninitialized + if not cfg.has_option(axis, field): + return 0xff + f = cfg.getint(axis, field) + if valid_range: + if f not in valid_range: + raise ValueError("bad value for %s" % field) + return f + + +# read a single axis from config file and return it as packed string +def validate_and_pack_axis_info(cfg, axis): + stick_id = get_field(cfg, axis, 'mapped_stick_id', range(5)) + # NB: direction - non-zero is Forward, 0 is Reverse + direction = get_field(cfg, axis, 'direction', [0, 1]) + expo = get_field(cfg, axis, 'expo') + return config_stick_axes_msg.pack_stick(stick_id, direction, expo) + + +# read a stick config file and return a message ready to send to STM32 +def msg_from_cfg(cfg): + msg = "" + for axis in ["stick-0", "stick-1", "stick-2", "stick-3", "stick-4", "stick-5"]: + msg += validate_and_pack_axis_info(cfg, axis) + return msg + + +# config_stick_axes.py set +# configure stick axes using the specified config file. +# +# config_stick_axes.py get +# read the stick axes configuration, and if they match the config file, return True + +if __name__ == "__main__": + + verbose = False # True for debug + + pp = pprint.PrettyPrinter() + + # two required arguments: set or get, and config file to use + if len(sys.argv) != 3: + usage() + sys.exit(1) + + if sys.argv[1] != "set" and sys.argv[1] != "get": + usage() + sys.exit(1) + + # read stick configuration + cfg = ConfigParser.ConfigParser() + try: + cfg.read(sys.argv[2]) + except: + print "%s: error opening %s" % (sys.argv[0], sys.argv[2]) + sys.exit(1) + + # convert stick configuration to "config stick axes" message + try: + request = msg_from_cfg(cfg) + except: + print "%s: error converting %s to request" % (sys.argv[0], sys.argv[2]) + sys.exit(1) + + if sys.argv[1] == "set": + # send message to stm32 + config_stick_axes_msg.send(request) + + # unpack request to dictionary for verification or compare below + request = config_stick_axes_msg.unpack(request) + if verbose: + print "request message:" + pp.pprint(request) + + # retrieve parameters + params = param_stored_vals_msg.fetch() + + # params is a string; unpack to a dictionary + params = param_stored_vals_msg.unpack(params) + if verbose: + print "params message:" + pp.pprint(params) + + # verify that the request matches the params read back + if request == params['rcSticks']: + match = True + else: + match = False + if sys.argv[1] == "set": + print "stick mapping returned does not match requested" + print "request:" + pp.pprint(request) + print "params:" + pp.pprint(params['rcSticks']) + sys.exit(1) + + if sys.argv[1] == "get": + if match: + print "true" + else: + print "false" + + sys.exit(0) diff --git a/flightcode/stm32/config_stick_axes_msg.py b/flightcode/stm32/config_stick_axes_msg.py new file mode 100644 index 0000000..992ee9b --- /dev/null +++ b/flightcode/stm32/config_stick_axes_msg.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python + +import socket +import struct + +# Must match flightcode/stm32 and/or config/sololink.orig +CONFIG_STICK_AXES_PORT = 5010 + +# Send message to STM32 via stm32 process +# +# Message is packed stick config data, but does not have the one-byte +# packet ID at the start. +def send(msg): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.sendto(msg, ("127.0.0.1", CONFIG_STICK_AXES_PORT)) + s.close() + +def pack_stick(stick_id, direction, expo): + return struct.pack(" " + print " set sweep time settings" + + +if __name__ == "__main__": + + verbose = False # True for debug + + pp = pprint.PrettyPrinter() + + # if two arguments, we are setting, and they are min and max + if len(sys.argv) == 1: + # getting + min_sweep_s = None + max_sweep_s = None + elif len(sys.argv) == 3: + # setting - arguments must be integers + try: + min_sweep_s = int(sys.argv[1]) + max_sweep_s = int(sys.argv[2]) + except: + usage() + sys.exit(1) + + if min_sweep_s is not None and max_sweep_s is not None: + # create "config sweep time" message + msg = config_sweep_time_msg.pack(min_sweep_s, max_sweep_s) + + # send message to stm32 + config_sweep_time_msg.send(msg) + + # retrieve parameters to verify + params = param_stored_vals_msg.fetch() + + # params is a string; unpack to a dictionary + params = param_stored_vals_msg.unpack(params) + if verbose: + print "params message:" + pp.pprint(params) + + if min_sweep_s is not None and max_sweep_s is not None: + # verify that sweep times are as requested + if params['sweepConfigs'][0] != (min_sweep_s, max_sweep_s): + print "requested sweep times (%d, %d); not returned in params" % \ + (min_sweep_s, max_sweep_s) + print "returned params:" + pp.pprint(params['sweepConfigs']) + sys.exit(1) + + print "%d %d" % (params['sweepConfigs'][0][0], params['sweepConfigs'][0][1]) + + sys.exit(0) diff --git a/flightcode/stm32/config_sweep_time_msg.py b/flightcode/stm32/config_sweep_time_msg.py new file mode 100644 index 0000000..c269bc4 --- /dev/null +++ b/flightcode/stm32/config_sweep_time_msg.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +import socket +import struct + +# Must match flightcode/stm32 and/or config/sololink.orig +CONFIG_SWEEP_TIME_PORT = 5022 + +# Send message to STM32 via stm32 process +# +# Message is packed sweep time data, but does not have the one-byte +# packet ID at the start. +def send(msg): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.sendto(msg, ("127.0.0.1", CONFIG_SWEEP_TIME_PORT)) + s.close() + +def pack(min_sweep_s, max_sweep_s): + return struct.pack(" next_print_time: + print "received:", str(msg) + next_print_time += print_interval + + if input_report_msg.msg_buf_long != 0: + print "input_report_msg.msg_buf_long=%d!" % input_report_msg.msg_buf_long + input_report_msg.msg_buf_long = 0 + + if input_report_msg.msg_buf_short != 0: + print "input_report_msg.msg_buf_short=%d!" % input_report_msg.msg_buf_short + input_report_msg.msg_buf_short = 0 diff --git a/flightcode/stm32/input_report_msg.py b/flightcode/stm32/input_report_msg.py new file mode 100644 index 0000000..bace93a --- /dev/null +++ b/flightcode/stm32/input_report_msg.py @@ -0,0 +1,82 @@ + +import struct + +""" +Input Report message as received from the controller's input report message +server. All fields are little-endian. + +Start +Byte Size Description +0 4 Message ID = 2003 +4 4 Length (bytes following) +8 8 Timestamp, usec since epoch +16 2 Gimbal Y +18 2 Gimbal Rate +20 2 Battery +22 2 Spare +24 (packet length) + +The timestamp is in microseconds since the Unix epoch. Note that it is set +when the message is first created on the controller, whose time is not +synchronized to anything in particular. At the moment it is only useful if +comparing timestamps to each other. +""" + +SOLO_MESSAGE_INPUT_REPORT = 2003 + +LENGTH = 24 + + +def unpack(s): + if len(s) != LENGTH: + return None + (msg_id, length, timestamp, gimbal_y, gimbal_rate, battery, spare) = \ + struct.unpack(" 0 and len(msg_buf) < LENGTH: + msg_buf_short = len(msg_buf) + elif len(msg_buf) > LENGTH: + msg_buf_long = len(msg_buf) + # got enough data yet? + if len(msg_buf) >= LENGTH: + # use the first LENGTH bytes in the buffer + msg = msg_buf[:LENGTH] + msg_buf = msg_buf[LENGTH:] + return unpack(msg) + else: + # not enough yet; wait for more + data = s.recv(1024) + if data is None: + # this case has not been observed to happen + continue + elif data == "": + # this happens if the other end closes + return None + else: + # append it to what we have then loop back and check for + # complete message + msg_buf = msg_buf + data + # end if + # end while diff --git a/flightcode/stm32/lockout_msg.py b/flightcode/stm32/lockout_msg.py new file mode 100755 index 0000000..8e05a28 --- /dev/null +++ b/flightcode/stm32/lockout_msg.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +# Show/hide "update required" screen. +# Required artoo 0.7.0 or later. + +import socket +import struct +import sys + +UNLOCK = 0 +LOCK = 1 + +# Must match flightcode/stm32 +LOCKOUT_STATE_PORT = 5020 + +def send(lock_state): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + msg = struct.pack("!B", lock_state) + s.sendto(msg, ("127.0.0.1", LOCKOUT_STATE_PORT)) + s.close() + +def send_lock(): + send(LOCK) + +def send_unlock(): + send(UNLOCK) + +def usage(): + print "usage: lockout_msg.py " + print " where is one of" + print " lock show \"update required\" screen" + print " unlock hide \"update required\" screen" + +if __name__ == "__main__": + # one required argument: "lock" or "unlock" + if len(sys.argv) != 2: + usage() + elif sys.argv[1] == "lock": + send_lock() + elif sys.argv[1] == "unlock": + send_unlock() + else: + usage() diff --git a/flightcode/stm32/packetTypes.h b/flightcode/stm32/packetTypes.h new file mode 100644 index 0000000..fb3cab3 --- /dev/null +++ b/flightcode/stm32/packetTypes.h @@ -0,0 +1,38 @@ +#ifndef _PACKETTYPES_H +#define _PACKETTYPES_H + +/*********************************************************************** +Serial packet IDs +Match constants in artoo/src/hostProtocol.h +***********************************************************************/ +#define PKT_ID_NOP 0 +#define PKT_ID_DSM_CHANNELS 1 +#define PKT_ID_CALIBRATE 2 +#define PKT_ID_SYS_INFO 3 +#define PKT_ID_MAVLINK 4 +#define PKT_ID_SET_RAW_IO 5 +#define PKT_ID_RAW_IO_REPORT 6 +#define PKT_ID_PAIR_REQUEST 7 +#define PKT_ID_PAIR_CONFIRM 8 +#define PKT_ID_PAIR_RESULT 9 +#define PKT_ID_SHUTDOWN_REQUEST 10 +#define PKT_ID_PARAM_STORED_VALS 11 +#define PKT_ID_OUTPUT_TEST 12 +#define PKT_ID_BUTTON_EVENT 13 +#define PKT_ID_INPUT_REPORT 14 +#define PKT_ID_CONFIG_STICK_AXES 15 +#define PKT_ID_BUTTON_FUNCTION_CFG 16 +#define PKT_ID_SET_SHOT_INFO 17 +#define PKT_ID_UPDATER 18 +#define PKT_ID_LOCKOUT_STATE 19 +#define PKT_ID_SELF_TEST 20 // test fixture only +#define PKT_ID_CONFIG_SWEEP_TIME 21 +#define PKT_ID_GPIO_TEST 22 +#define PKT_ID_TEST_EVENT 23 +#define PKT_ID_SET_TELEM_UNITS 24 +#define PKT_ID_INVALID_STICK_INPUTS 25 +#define PKT_ID_SOLO_APP_CONNECTION 26 + +#define PKT_ID_MAX 27 + +#endif //_PACKETTYPES_H diff --git a/flightcode/stm32/param_stored_vals_msg.py b/flightcode/stm32/param_stored_vals_msg.py new file mode 100755 index 0000000..32a1a0f --- /dev/null +++ b/flightcode/stm32/param_stored_vals_msg.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python + +# Send PARAM_STORED_VALS message +# +# The request is the message ID only +# The response is a complex structure (not handled here) + +##import datetime +import socket +import struct +import sys +import pprint + +# must match flightcode/stm32 +PARAM_STORED_VALS_PORT = 5011 + +def send(): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.sendto("", ("127.0.0.1", PARAM_STORED_VALS_PORT)) + s.close() + +##max_delay = 0.0 + +# send request, wait for response +def fetch(): + ##global max_delay + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + # Most of the time we get a response in <17 msec. + # In one run of 10,000 fetches, the longest delay was <82 msec. + s.settimeout(0.2) + # send request + ##start_time = datetime.datetime.now() + s.sendto("", ("127.0.0.1", PARAM_STORED_VALS_PORT)) + # wait for response + try: + msg = s.recv(256) + except: + msg = "" + ##else: + ## delta_time = datetime.datetime.now() - start_time + ## delta_time = delta_time.total_seconds() + ## if max_delay < delta_time: + ## max_delay = delta_time + ## print "response in %f sec" % (delta_time, ) + s.close() + return msg + +# parse string msg to dictionary +# +# { +# 'stickCals' : [ stickCal0, ... stickCal5 ], +# 'presets' : [ preset0, preset1 ], +# 'rcSticks' : [ rcStick0, ... rcStick5 ], +# 'buttonConfigs' : [ buttonConfig0, ... buttonConfig2 ], +# 'sweepConfig' : [ sweepConfig0 ] +# } +# +# stickCalN = +# ( +# , # minVal +# , # trim +# # maxVal +# ) +# presetN = +# ( +# +# ) +# rcStickN = +# ( +# , # input +# , # direction +# # expo +# ) +# buttonConfigN = +# ( +# , # buttonID +# , # buttonEvt +# , # shotID +# , # state +# # descriptor +# ) +# sweepConfigN = +# ( +# , # minSweepSec +# # maxSweepSec +# ) +def unpack(msg): + x = 0 + + stickCals = [] + for i in range(6): + stickCal = struct.unpack(" +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" +#include "mutex.h" +#include "RC.h" +#include "Telem.h" +#include "SysInfo.h" +#include "PairReq.h" +#include "PairRes.h" +#include "ParamStoredVals.h" +#include "ConfigStickAxes.h" +#include "SetTelemUnits.h" +#include "ConfigSweepTime.h" +#include "ButtonEventHandler.h" +#include "InputReport.h" +#include "ButtonFunctionCfg.h" +#include "SetShotInfo.h" +#include "Updater.h" +#include "LockoutState.h" +#include "AppConnected.h" +#include "SLIP.h" +#include "packetTypes.h" +#include "INIReader.h" +#include "net_wmm.h" +#ifdef INCLUDE_SERIAL_LOG +#include "SerialLog.h" +#endif // INCLUDE_SERIAL_LOG + +using namespace std; + +/*********************************************************************** +Timeouts +***********************************************************************/ +#define LOG_DT_US 10000000 // 10s + +/*********************************************************************** +Serial port name +***********************************************************************/ +string serialPortName; + +/*********************************************************************** +Serial port receive buffer +***********************************************************************/ +#define SERIAL_BUFSIZE 4096 +#define MAX_SERIAL_MESSAGE_LEN 1024 + +/*********************************************************************** +Thread IDs +***********************************************************************/ +pthread_t upstream_ctx; +pthread_t downstream_ctx; + +/*********************************************************************** +Thread priorities +***********************************************************************/ +#define UPSTREAM_PRIORITY 60 +#define DOWNSTREAM_PRIORITY 56 + +/*********************************************************************** +File descriptors +***********************************************************************/ +int serial_fd; + +#ifdef INCLUDE_SERIAL_LOG +/*********************************************************************** +Serial port log +***********************************************************************/ +SerialLog *serialLog; +unsigned serialLogSize; +unsigned serialLogTimeout_us; +#endif // INCLUDE_SERIAL_LOG + +/*********************************************************************** +The amount of data sent/received every cycle +***********************************************************************/ +unsigned upBytes = 0; +unsigned upMsgs[PKT_ID_MAX + 1]; +pthread_mutex_t mutex_upBytes; + +/*********************************************************************** +Handler objects +***********************************************************************/ +RC *rc; +Telem *telem; +SysInfo *sysInfo; +PairReq *pairReq; +PairRes *pairRes; +ParamStoredVals *paramStoredVals; +ConfigStickAxes *configStickAxes; +SetTelemUnits *setTelemUnits; +ConfigSweepTime *configSweepTime; +ButtonEventHandler *btnEvt; +InputReport *inputReport; +ButtonFunctionCfg *btnFuncCfg; +SetShotInfo *setShotInfo; +Updater *updater; +LockoutState *lockoutState; +AppConnected *appConn; + +/*********************************************************************** +Debug control +***********************************************************************/ +static uint32_t dbg_downHandler = 0; + +/*********************************************************************** +Function: void lock_thread_mem(uint32_t memsize) + +Description: Locks memory in the thread so that stack faults don't cause + timing jitter. + ***********************************************************************/ +static void lock_thread_mem(uint32_t memsize) +{ + uint8_t stackmem[memsize]; + + mlockall(MCL_CURRENT | MCL_FUTURE); + + memset(stackmem, 0, sizeof(stackmem)); +} + +/*********************************************************************** +Function: int serial_setup(int baudrate) + +Description: The serial port initialization function. This function + initializes the serial port over which data is sent and + received from the STM32. A return of 0 indicates an error. +***********************************************************************/ +int serial_setup(int baudrate) +{ + struct termios options; + + serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY); // Blocking read + + if (serial_fd < 0) { + syslog(LOG_ERR, "unable to open serial port %s", serialPortName.c_str()); + return 0; + } + + // Configure port for 8N1 transmission + tcgetattr(serial_fd, &options); // Gets the current options for the port + switch (baudrate) { + case 57600: + cfsetspeed(&options, B57600); + break; + case 115200: + cfsetspeed(&options, B115200); + break; + case 1500000: + cfsetspeed(&options, B1500000); + break; + default: + syslog(LOG_ERR, "unsupported baudrate %d", baudrate); + return 0; + } + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; + options.c_cc[VMIN] = 1; + options.c_cc[VTIME] = 0; + + tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW" + + syslog(LOG_INFO, "opened serial port %s", serialPortName.c_str()); + + return 1; +} + +static string msgToString(const char *msg, int msgLen) +{ + stringstream s; + + // dealing with the unknown; cap the number of bytes to log + int printLen = msgLen; + if (printLen > 8) + printLen = 8; + + while (printLen-- > 0) { + s << setfill('0') << setw(2) << hex << unsigned(*msg); + if (printLen > 0) + s << " "; + msg++; + } + + s << setfill(' ') << setw(0) << dec; + + if (msgLen > 8) + s << "... (" << msgLen << " bytes long)"; + + return s.str(); +} + +/*********************************************************************** +Function: void *upstream_task(void*) + +Description: The upstream thread task. Checks for data available on the + serial and calls the relevant handler. +***********************************************************************/ +const int POLLTIMEOUT_MS = 10000; +void *upstream_task(void *) +{ + char buf[SERIAL_BUFSIZE]; + char msg[MAX_SERIAL_MESSAGE_LEN]; + struct pollfd fds[1]; + int pollrc; + int i; + int msgLen = 0; + int readlen; + SLIPDecoder *slipDec = new SLIPDecoder(msg, sizeof(msg)); + uint64_t now_us; + uint64_t unknown_us = 0; + unsigned unknown_skip_cnt = 0; + bool in_sync = false; + bool invalid_stick_inputs_logged = false; + + fds[0].fd = serial_fd; + fds[0].events = POLLIN; + + lock_thread_mem(32764); + + while (true) { + // Wait for available data, block until we get some or timeout + pollrc = poll(fds, 1, POLLTIMEOUT_MS); + + if (pollrc > 0 && fds[0].revents & POLLIN) { + // Check for any new incoming data, send over udp + readlen = read(serial_fd, buf, SERIAL_BUFSIZE); + if (readlen > 0) { + for (i = 0; i < readlen; ++i) { + // Decode slip packets. Returns: + // 0 : packet in progress + // -1 : error in bytes stream, e.g. invalid sync sequence + // >0 : packet received + msgLen = slipDec->addByte(&buf[i]); + if (msgLen <= 0) { + if (msgLen < 0) { + syslog(LOG_INFO, "slip error - discarding data"); + in_sync = false; + } + continue; + } + + if (!in_sync) { + // First packet after an error (or init) is no good, + // since we generally did not start after a previous + // packet's sync. But at this point, we *did* just + // receive a sync, so we drop the current packet and + // go get the next one. + syslog(LOG_INFO, "slip - initial sync"); + in_sync = true; + continue; + } + +// OK, got a message + +#ifdef INCLUDE_SERIAL_LOG + serialLog->log_packet(msg, msgLen, SerialLog::PKTFLG_UP); +#endif // INCLUDE_SERIAL_LOG + + // syslog(LOG_INFO, "packet %d: %d bytes", int(msg[0]), msgLen); + + // Get the packet type from the first byte + // When passing the message, skip the first byte + // which is the packet type. + unsigned up = 0; + switch (msg[0]) { + case PKT_ID_DSM_CHANNELS: + up = rc->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_CALIBRATE: + break; + case PKT_ID_SYS_INFO: + up = sysInfo->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_MAVLINK: + up = telem->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_PAIR_CONFIRM: + up = pairReq->upHandler(&msg[1], msgLen - 1); + break; + // PKT_ID_PAIR_RESULT is never upstream + case PKT_ID_PARAM_STORED_VALS: + up = paramStoredVals->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_SHUTDOWN_REQUEST: + // Call the system shutdown command + syslog(LOG_INFO, "STM32 requested shutdown"); + system("shutdown -h now"); + sleep(30); // should get killed in here + exit(1); // init may restart us if we get here + break; + case PKT_ID_BUTTON_EVENT: + up = btnEvt->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_INPUT_REPORT: + up = inputReport->upHandler(&msg[1], msgLen - 1); + break; + case PKT_ID_INVALID_STICK_INPUTS: + // polluting log - print one then ignore + if (!invalid_stick_inputs_logged) { + string s = msgToString(msg, msgLen); + syslog(LOG_INFO, "received invalid stick inputs message: %s", + s.c_str()); + invalid_stick_inputs_logged = true; + } + break; + case PKT_ID_NOP: + default: + // Print unknown messages, but max one per second + // Rate limit so if the STM32 firmware gets ahead + // and emits a new message, we don't soak the log. + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (now_us >= unknown_us) { + // okay to print it + string s = msgToString(msg, msgLen); + syslog(LOG_ERR, "unknown packet: %s", s.c_str()); + if (unknown_skip_cnt != 0) { + syslog(LOG_ERR, "(%u more not printed)", unknown_skip_cnt); + unknown_skip_cnt = 0; + } + // don't print another for 1,000,000 usec + unknown_us = now_us + 1000000; + } else { + // too soon after the previous one; don't print + unknown_skip_cnt++; + } + break; + } + unsigned id = msg[0]; + if (id > PKT_ID_MAX) + id = PKT_ID_MAX; + pthread_mutex_lock(&mutex_upBytes); + upMsgs[id]++; // upMsgs is [PKT_ID_MAX + 1] + upBytes += up; + pthread_mutex_unlock(&mutex_upBytes); + } + } + } else if (pollrc == 0) { + // timeout + syslog(LOG_INFO, "no data received from STM32 for %d msec", POLLTIMEOUT_MS); + } + + } // while (true) + + pthread_exit(NULL); +} + +/********************************************************************** +Function: int start_upstream_thread(void) + +Description: Starts the upstream thread +***********************************************************************/ +int start_upstream_thread(void) +{ + int ret = 0; + struct sched_param param; + + // Start the upstream thread + pthread_create(&upstream_ctx, NULL, upstream_task, NULL); + + pthread_setname_np(upstream_ctx, "stm32_up"); + + // upstream priority and schedule + param.sched_priority = UPSTREAM_PRIORITY; + if ((ret = pthread_setschedparam(upstream_ctx, SCHED_FIFO, ¶m))) + syslog(LOG_ERR, "error %d setting upstream thread priority", ret); + + return ret; +} + +/********************************************************************** +Function: void *downstream_task(void*) + +Description: The downstream thread task. Selects() on the set of + fds and calls the relevant handler +***********************************************************************/ +void *downstream_task(void *) +{ + int nfds = 0; + int res = 0; + fd_set fds; + int bytesAvailable; + struct timeval timeout; + uint64_t log_last_us; + unsigned downBytes = 0; + + lock_thread_mem(32764); + + log_last_us = clock_gettime_us(CLOCK_MONOTONIC); + + // Select() on the list of UDP FDs. + while (1) { + // Block until we get one + // Generate the list of fds + FD_ZERO(&fds); + FD_SET(telem->getfd(), &fds); + nfds = telem->getfd() + 1; + + FD_SET(pairReq->getfd(), &fds); + if (pairReq->getfd() >= nfds) + nfds = pairReq->getfd() + 1; + + FD_SET(pairRes->getfd(), &fds); + if (pairRes->getfd() >= nfds) + nfds = pairRes->getfd() + 1; + + FD_SET(sysInfo->getfd(), &fds); + if (sysInfo->getfd() >= nfds) + nfds = sysInfo->getfd() + 1; + + FD_SET(paramStoredVals->getfd(), &fds); + if (paramStoredVals->getfd() >= nfds) + nfds = paramStoredVals->getfd() + 1; + + FD_SET(configStickAxes->getfd(), &fds); + if (configStickAxes->getfd() >= nfds) + nfds = configStickAxes->getfd() + 1; + + FD_SET(setTelemUnits->getfd(), &fds); + if (setTelemUnits->getfd() >= nfds) + nfds = setTelemUnits->getfd() + 1; + + FD_SET(configSweepTime->getfd(), &fds); + if (configSweepTime->getfd() >= nfds) + nfds = configSweepTime->getfd() + 1; + + FD_SET(btnFuncCfg->getfd(), &fds); + if (btnFuncCfg->getfd() >= nfds) + nfds = btnFuncCfg->getfd() + 1; + + FD_SET(setShotInfo->getfd(), &fds); + if (setShotInfo->getfd() >= nfds) + nfds = setShotInfo->getfd() + 1; + + FD_SET(updater->getfd(), &fds); + if (updater->getfd() >= nfds) + nfds = updater->getfd() + 1; + + FD_SET(lockoutState->getfd(), &fds); + if (lockoutState->getfd() >= nfds) + nfds = lockoutState->getfd() + 1; + + FD_SET(appConn->getfd(), &fds); + if (appConn->getfd() >= nfds) + nfds = appConn->getfd() + 1; + + // select timeout 0.1 sec + // (note that select can update timeout) + // need to time out, so if downlink stops we still see stats + timeout.tv_sec = 0; + timeout.tv_usec = 100000; + + res = select(nfds, &fds, NULL, NULL, &timeout); + + if (res > 0) { + + // Run the appropriate downstream handler + if (FD_ISSET(telem->getfd(), &fds)) { + // record the amount of data we download + ioctl(telem->getfd(), FIONREAD, &bytesAvailable); + + // Run the handler + telem->downHandler(serial_fd); + + // Record the amount of data we got down + downBytes += bytesAvailable; + } + + if (FD_ISSET(pairReq->getfd(), &fds)) { + // Don't increment the amount of data we send here + syslog(LOG_INFO, "pair request going down"); + pairReq->downHandler(serial_fd, dbg_downHandler); + } + + if (FD_ISSET(pairRes->getfd(), &fds)) { + // Don't increment the amount of data we send here + syslog(LOG_INFO, "pair result going down"); + pairRes->downHandler(serial_fd, dbg_downHandler); + } + + if (FD_ISSET(sysInfo->getfd(), &fds)) { + // Don't increment the amount of data we send here + sysInfo->downHandler(serial_fd, dbg_downHandler); + } + + if (FD_ISSET(paramStoredVals->getfd(), &fds)) + paramStoredVals->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(configStickAxes->getfd(), &fds)) + configStickAxes->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(setTelemUnits->getfd(), &fds)) + setTelemUnits->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(configSweepTime->getfd(), &fds)) + configSweepTime->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(btnFuncCfg->getfd(), &fds)) + btnFuncCfg->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(setShotInfo->getfd(), &fds)) + setShotInfo->downHandler(serial_fd, dbg_downHandler); + + if (FD_ISSET(updater->getfd(), &fds)) { + // Don't increment the amount of data we send here + syslog(LOG_INFO, "updater message going down"); + updater->downHandler(serial_fd, dbg_downHandler); + } + + if (FD_ISSET(lockoutState->getfd(), &fds)) { + // Don't increment the amount of data we send here + syslog(LOG_INFO, "lockout state message going down"); + lockoutState->downHandler(serial_fd, dbg_downHandler); + } + + if (FD_ISSET(appConn->getfd(), &fds)) { + // Don't increment the amount of data we send here + syslog(LOG_INFO, "app connected message going down"); + appConn->downHandler(serial_fd, dbg_downHandler); + } + } + + // check for time to log status logging + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + uint64_t delta = now_us - log_last_us; + if (delta >= LOG_DT_US) { + unsigned newUpBytes; + unsigned newUpMsgs[PKT_ID_MAX + 1]; + pthread_mutex_lock(&mutex_upBytes); + newUpBytes = upBytes; + upBytes = 0; + memcpy(newUpMsgs, upMsgs, sizeof(newUpMsgs)); + memset(upMsgs, 0, sizeof(upMsgs)); + pthread_mutex_unlock(&mutex_upBytes); + + log_last_us = now_us; + + // normally need about 20 chars tops + char msg[200]; + int x = sizeof(msg) - 1; + char *p = msg; + memset(msg, 0, sizeof(msg)); + for (int i = 0; i <= PKT_ID_MAX; i++) { + if (newUpMsgs[i] != 0) { + int k = snprintf(p, x, "%d:%u ", i, newUpMsgs[i]); + if (k >= x) + break; + x -= k; + p += k; + } + } + + syslog(LOG_INFO, "up %s%u B %0.2f KB/s dn %u B %0.2f KB/s", msg, newUpBytes, + newUpBytes / 1024.0 / (delta / 1e6), downBytes, + downBytes / 1024.0 / (delta / 1e6)); + + downBytes = 0; + } + } + + pthread_exit(NULL); +} + +/********************************************************************** +Function: int start_downstream_thread(void) + +Description: Starts the downstream thread +***********************************************************************/ +int start_downstream_thread(void) +{ + int ret = 0; + struct sched_param param; + + // Start the downstream thread + pthread_create(&downstream_ctx, NULL, downstream_task, NULL); + + pthread_setname_np(downstream_ctx, "stm32_down"); + + // Set the priority and scheduler + memset(¶m, 0, sizeof(param)); + param.sched_priority = DOWNSTREAM_PRIORITY; + if ((ret = pthread_setschedparam(downstream_ctx, SCHED_FIFO, ¶m))) + syslog(LOG_ERR, "error %d setting downstream thread priority", ret); + + return ret; +} + +/********************************************************************** +Function: bool readSoloIP(string filename, string *ip) + +Description: Reads the Solo's IP address from the solo Address File +***********************************************************************/ +bool readSoloIP(string filename, string *ip) +{ + ifstream fd; + + // Open the address file, return false if its not been created yet + fd.open(filename.c_str(), ios::in); + if (!fd) + return false; + + ip->clear(); + fd >> *ip; + fd.close(); + + return true; +} + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(void) +{ + int rcDestPort; + int rcUpTos; + int mavDestPort; + int sysDestPort; + int pairReqDestPort; + int pairResDestPort; + int paramStoredValsDestPort; + int configStickAxesDestPort; + int setTelemUnitsDestPort; + string setTelemUnitsSetting; + int telemLogGap; + int telemLogDelayMax; + string telemLogDelayFile; + int configSweepTimeDestPort; + int btnEvtDestPort; + int inputReportDestPort; + int btnFuncCfgDestPort; + int setShotInfoDestPort; + int updaterDestPort; + int lockoutStateDestPort; + int appConnDestPort; + int baudrate; + string soloIPAddr; + bool haveSoloIP = false; + + openlog("stm32", LOG_NDELAY, LOG_LOCAL0); + + syslog(LOG_INFO, "main: starting: built " __DATE__ " " __TIME__); + + /* Parse the sololink.conf file for serial port, source IPs and ports */ + INIReader reader("/etc/sololink.conf"); + + if (reader.ParseError() < 0) { + syslog(LOG_ERR, "main: can't load /etc/sololink.conf"); + return -1; + } + + /* Get all parameter info */ + serialPortName = reader.Get("solo", "stm32Dev", "/dev/ttymxc1"); + rcDestPort = reader.GetInteger("solo", "rcDestPort", 5005); + rcUpTos = reader.GetInteger("solo", "rcUpTos", 0xff); + mavDestPort = reader.GetInteger("solo", "mavDestPort", 5015); + sysDestPort = reader.GetInteger("solo", "sysDestPort", 5012); + pairReqDestPort = reader.GetInteger("solo", "pairReqDestPort", 5013); + pairResDestPort = reader.GetInteger("solo", "pairResDestPort", 5014); + paramStoredValsDestPort = reader.GetInteger("solo", "paramStoredValsDestPort", 5011); + configStickAxesDestPort = reader.GetInteger("solo", "configStickAxesDestPort", 5010); + setTelemUnitsDestPort = reader.GetInteger("solo", "setTelemUnitsDestPort", 5024); + setTelemUnitsSetting = reader.Get("solo", "uiUnits", "metric"); + telemLogGap = reader.GetInteger("solo", "telemLogGap", 1000000); + syslog(LOG_INFO, "logging telemetry gaps >= %d msec", (telemLogGap + 500) / 1000); + telemLogDelayMax = + reader.GetInteger("solo", "telemLogDelayMax", 100000); // 100,000 is ~20 minutes, 6.4 MB + telemLogDelayFile = reader.Get("solo", "telemLogDelayFile", ""); + if (telemLogDelayFile != "") + syslog(LOG_INFO, "main: logging packet delays to %s (%d records per file)", + telemLogDelayFile.c_str(), telemLogDelayMax); + configSweepTimeDestPort = reader.GetInteger("solo", "configSweepTimeDestPort", 5022); + btnEvtDestPort = reader.GetInteger("solo", "buttonEventDestPort", 5016); + inputReportDestPort = reader.GetInteger("solo", "inputReportDestPort", 5021); + btnFuncCfgDestPort = reader.GetInteger("solo", "buttonFunctionConfigDestPort", 5017); + setShotInfoDestPort = reader.GetInteger("solo", "setShotInfoDestPort", 5018); + updaterDestPort = reader.GetInteger("solo", "updaterDestPort", 5019); + lockoutStateDestPort = reader.GetInteger("solo", "lockoutStateDestPort", 5020); + appConnDestPort = reader.GetInteger("solo", "appConnDestPort", 5026); + baudrate = reader.GetInteger("solo", "stm32Baud", 115200); + dbg_downHandler = reader.GetInteger("solo", "dbg_downHandler", 0); + if (dbg_downHandler != 0) + syslog(LOG_INFO, "main: dbg_downHandler=0x%08x", dbg_downHandler); + +#ifdef INCLUDE_SERIAL_LOG + // serial log + serialLogSize = reader.GetInteger("solo", "serialLogSize", 50000); + serialLogTimeout_us = reader.GetInteger("solo", "serialLogTimeout_us", 1000000); + + serialLog = new SerialLog(serialLogSize, serialLogTimeout_us); +#endif // INCLUDE_SERIAL_LOG + + // shared variable mutex + if (mutex_init(&mutex_upBytes) != 0) { + syslog(LOG_ERR, "main: can't initialize mutex"); + return -1; + } + + // initialize all the handler objects + + // The RC destination address gets updated by the Solo IP address file. We don't + // send any data to the Solo until we've got that address + rc = new RC("", rcDestPort, rcUpTos); + telem = new Telem("127.0.0.1", mavDestPort, telemLogGap, telemLogDelayMax, telemLogDelayFile); + sysInfo = new SysInfo("127.0.0.1", sysDestPort); + pairReq = new PairReq("127.0.0.1", pairReqDestPort); + pairRes = new PairRes("127.0.0.1", pairResDestPort); + paramStoredVals = new ParamStoredVals(paramStoredValsDestPort); + configStickAxes = new ConfigStickAxes(configStickAxesDestPort); + setTelemUnits = new SetTelemUnits(setTelemUnitsDestPort); + configSweepTime = new ConfigSweepTime(configSweepTimeDestPort); + btnEvt = new ButtonEventHandler(btnEvtDestPort); + inputReport = new InputReport(inputReportDestPort, 1); + btnFuncCfg = new ButtonFunctionCfg(btnFuncCfgDestPort); + setShotInfo = new SetShotInfo(setShotInfoDestPort); + updater = new Updater("127.0.0.1", updaterDestPort); + lockoutState = new LockoutState(lockoutStateDestPort); + appConn = new AppConnected("127.0.0.1", appConnDestPort); + + /* Start the serial port */ + if (!serial_setup(baudrate)) { + syslog(LOG_ERR, "main: unable to initialize serial port"); + return -1; + } + + /* Start the upstream and downstream threads */ + start_downstream_thread(); + start_upstream_thread(); + + // Send a ping to get the STM32 information in the log + sysInfo->ping(); + + // Set the telemetry display units + setTelemUnits->set(setTelemUnitsSetting); + + // The main while() loop just becomes a thread monitor. If any thread dies, + // exit and let inittab restart us + // We also check to see if the solo IP address has been written yet + while (true) { + if (pthread_kill(upstream_ctx, 0) != 0 || pthread_kill(downstream_ctx, 0) != 0) { + syslog(LOG_ERR, "a thread is not running, exiting"); + exit(0); + } + + // Check the Solo IP address file + if (!haveSoloIP) { + haveSoloIP = readSoloIP("/var/run/solo.ip", &soloIPAddr); + if (haveSoloIP) { + syslog(LOG_INFO, "got solo ip: %s", soloIPAddr.c_str()); + rc->setSoloIP(&soloIPAddr); + } + } + + // We also "ping" the STM32 occasionally to make sure its still talking to us. + sysInfo->ping(); + + // Long sleep + sleep(1); + } + + return -1; +} diff --git a/flightcode/stm32/updater_msg.py b/flightcode/stm32/updater_msg.py new file mode 100755 index 0000000..ece868d --- /dev/null +++ b/flightcode/stm32/updater_msg.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +# Show update status screens: updating system, update complete, update failed + +import socket +import struct +import sys + +START = 0 +SUCCESS = 1 +FAIL = 2 + +# must match flightcode/stm32 +UPDATER_PORT = 5019 + +def send(id): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + msg = struct.pack("!B", id) + s.sendto(msg, ("127.0.0.1", UPDATER_PORT)) + s.close() + +def send_start(): + send(START) + +def send_success(): + send(SUCCESS) + +def send_fail(): + send(FAIL) + +def usage(): + print "usage: updater_msg.py " + print " where is one of" + print " start show \"updating system\" screen" + print " success show \"update complete\" screen" + print " fail show \"update failed\" screen" + +if __name__ == "__main__": + # one required argument: "start", "success", or "fail" + if len(sys.argv) != 2: + usage() + elif sys.argv[1] == "start": + send_start() + elif sys.argv[1] == "success": + send_success() + elif sys.argv[1] == "fail": + send_fail() + else: + usage() diff --git a/flightcode/telem/Makefile b/flightcode/telem/Makefile new file mode 100644 index 0000000..1210d34 --- /dev/null +++ b/flightcode/telem/Makefile @@ -0,0 +1,48 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../log ../ini ../ini/cpp + +INCS = -I../util -I../log -I../ini -I../ini/cpp + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS = -lpthread + +SRCS_CPP = telem_forward.cpp +SRCS_CPP += RcLock.cpp +SRCS_CPP += INIReader.cpp Log.cpp +SRCS_C = util.c ini.c +SRCS_C += file_util.c +SRCS_C += syslog.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = telem_forwarder + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/telem/telem_forward.cpp b/flightcode/telem/telem_forward.cpp new file mode 100644 index 0000000..02f5b2c --- /dev/null +++ b/flightcode/telem/telem_forward.cpp @@ -0,0 +1,1458 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "RcLock.h" +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "../mavlink/c_library/common/mavlink.h" +#include "../ini/cpp/INIReader.h" +#include +#include +#include "link_packet.h" +#include "net_wmm.h" + +/*********************************************************************** + +This is the module that runs on Solo, relaying telemetry between the +Pixhawk and Artoo. + +There is also a mechanism for other software on Solo to connect to +the telemetry stream. + +***********************************************************************/ + +using namespace std; + +/*********************************************************************** +Timeouts +***********************************************************************/ +#define LOG_DT_US 10000000 // 10s +#define CLIENT_TIMEOUT_US 10000000 // 10s +#define SEND_DT_US 100000 // 100ms + +/*********************************************************************** +UDP macros +***********************************************************************/ +#define UDP_PORT 14560 // The incoming UDP port +#define BUFSIZE 4096 + +/*********************************************************************** +Serial port name +***********************************************************************/ +string serialPortName; + +/*********************************************************************** +GPS time usage control +***********************************************************************/ +static bool useGpsTime = true; +static bool gotGpsTime = false; + +/*********************************************************************** +Thread IDs +***********************************************************************/ +pthread_t upstream_ctx; +pthread_t downstream_ctx; +pthread_t logging_ctx; + +static pthread_mutex_t mutex_upStats; +static pthread_mutex_t mutex_downStats; + +/*********************************************************************** +Thread priorities +Downstream is naturally rate-limited by receiving messages from the +UART (~200 usec for a 20 byte message at 921k baud). To avoid letting +an upstream storm block downstream messages, we make downstream +higher priority. A better arrangement might be to use a single thread +servicing both directions. +***********************************************************************/ +#define UPSTREAM_PRIORITY 54 +#define DOWNSTREAM_PRIORITY 55 +#define LOG_PRIORITY 3 + +/*********************************************************************** +File descriptors +***********************************************************************/ +int sock_fd; +int serial_fd; +int inject_fd = -1; + +/*********************************************************************** +Log control (debug) +A bitmask, where each bit might enable certain things to the log file. +***********************************************************************/ +static uint32_t logControl = 0; +#define LOG_CONTROL_MSG_COUNTS_DOWN 0x00000001 + +/*********************************************************************** +List of ip/ports to send data to +***********************************************************************/ +struct sockdata { + struct sockaddr_in sa; + uint64_t lastRecvTime_us; +}; + +// e.g. { 37 => { 23 => client1, 56 => client2 }, .... } +// (seen system 37, components 23 and 56 from different clients) +pthread_mutex_t mutex_arp_table; +std::map< uint8_t, std::map< uint8_t, sockdata * > > arp_table; + +list< sockdata * > clients; + +/*********************************************************************** +The artoo addr, which we always send to +***********************************************************************/ +struct sockaddr_in artooaddr; + +/*********************************************************************** +Mutex for the client data + +It is expected that only the upstream thread modifies the client list. +This allows the upstream thread to read the client list without +acquiring the mutex. +***********************************************************************/ +pthread_mutex_t mutex_clients; + +/*********************************************************************** +The amount of data sent/received every cycle +***********************************************************************/ + +// from either pixhawk or injection port +static unsigned downMsgs = 0; +static unsigned downBytes = 0; + +// from uplink UDP socket +static unsigned upMsgs = 0; +static unsigned upBytes = 0; + +static unsigned upBadShort = 0; +static unsigned upBadMagic = 0; +static unsigned upBadLength = 0; + +// specifically through the pixhawk serial interface +static unsigned serialDownBytes = 0; +static unsigned serialUpBytes = 0; + +int serialBaud; +bool serialFlow = true; + +// by mavlink ID +static const int ID_MIN = 0; +static const int ID_MAX = 256; +static unsigned downMsgsById[ID_MAX]; + +/* sigquit handling; we should probably join the threads and whatnot... */ +void sigquit_handler(int signum) +{ + exit(0); +} + +/*********************************************************************** + rc lockout +************************************************************************/ +static bool rc_locked = false; + +/*********************************************************************** +Stream state + +Monitor (downstream) message flows to detect sequence errors (dropped +messages). This is diagnostic only. Sequence is maintained per (sysid, +compid) pair. + +We allow for unknown messages (those we cannot decode but will forward). +To prevent garbage data from tricking us into creating stream states +for lots of junk streams, we require a known message from a (sysid, +compid) in order to create a stream state for it. After that, we'll +accept unknown messages with that same (sysid, compid) as we check +the sequence for gaps. +***********************************************************************/ + +struct stream_state { + uint8_t sysid; + uint8_t compid; + uint8_t seq; + + unsigned seq_err; + + stream_state(uint8_t sysid, uint8_t compid, uint8_t seq = 0) + : sysid(sysid), compid(compid), seq(seq), seq_err(0) + { + } +}; + +// All streams we have seen messages for +list< stream_state > streams; + +pthread_mutex_t mutex_streams; + +/*********************************************************************** +Function: int UDP_setup(void) + +Description: Sets up the UDP port used for both upstream and + downstream telemetry. Returns 0 in the event of an + error, 1 otherwise. +***********************************************************************/ +int UDP_setup(int tos) +{ + struct sockaddr_in myaddr; + struct timeval timeout; + + /* create a UDP socket */ + if ((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + syslog(LOG_ERR, "can't create socket"); + return 0; + } + + /* Socket timeout */ + timeout.tv_sec = 1; + timeout.tv_usec = 0; + if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout)) != 0) { + syslog(LOG_ERR, "setsockopt (SO_RCVTIMEO) failed"); + return 0; + } + + /* bind the socket to any valid IP address and a specific port */ + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + + /* Accept a connection from any IP. */ + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(UDP_PORT); + + if (bind(sock_fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + syslog(LOG_ERR, "bind failed"); + return 0; + } + + if (setsockopt(sock_fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) != 0) { + syslog(LOG_ERR, "setsockopt (IP_TOS) failed"); + return 0; + } + + syslog(LOG_INFO, "opened port %d", UDP_PORT); + return 1; +} + +/*********************************************************************** +Function: int inject_setup(const char *sock_name) + +Description: Initialize the unix-domain telemetry injection port. + Anything sent to this port is injected into the down- + bound telemetry stream. Packets written to this port are + expected to be MAVLink packets, ready to copy into the + stream. Returns 1 on success, 0 on error. +***********************************************************************/ +int inject_setup(const char *sock_name) +{ + struct sockaddr_un un; + + if (strlen(sock_name) >= sizeof(un.sun_path)) { + syslog(LOG_ERR, "inject socket name too long"); + return 0; + } + + // If there is something already there with the socket name, it is + // deleted. If not, unlink() fails (and we don't care). + (void)unlink(sock_name); + + // create socket + inject_fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (inject_fd < 0) { + syslog(LOG_ERR, "creating inject socket"); + return 0; + } + + // bind inject socket to address + memset(&un, 0, sizeof(un)); + un.sun_family = AF_UNIX; + strncpy(un.sun_path, sock_name, sizeof(un.sun_path) - 1); + if (bind(inject_fd, (struct sockaddr *)&un, sizeof(un)) != 0) { + syslog(LOG_ERR, "binding inject socket"); + close(inject_fd); + inject_fd = -1; + return 0; + } + + return 1; +} + +/*********************************************************************** +Function: int serial_setup(int baud) + +Description: The serial port initialization function. This function + initializes the serial port over which telemtry is sent + to and received from the Pixhawk. A return of 0 indicates + an error. +***********************************************************************/ +int serial_setup(int baud) +{ + struct termios options; + + serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (serial_fd < 0) { + syslog(LOG_ERR, "unable to open serial port %s", serialPortName.c_str()); + return 0; + } + + // Configure port for 8N1 transmission + tcgetattr(serial_fd, &options); // Gets the current options for the port + // Set the output baud rate + switch (baud) { + case 1200: + cfsetspeed(&options, B1200); + break; + case 2400: + cfsetspeed(&options, B2400); + break; + case 4800: + cfsetspeed(&options, B4800); + break; + case 9600: + cfsetspeed(&options, B9600); + break; + case 19200: + cfsetspeed(&options, B19200); + break; + case 38400: + cfsetspeed(&options, B38400); + break; + case 57600: + cfsetspeed(&options, B57600); + break; + case 115200: + cfsetspeed(&options, B115200); + break; + case 500000: + cfsetspeed(&options, B500000); + break; + case 921600: + cfsetspeed(&options, B921600); + break; + case 1500000: + cfsetspeed(&options, B1500000); + break; + default: + syslog(LOG_ERR, "unsupported baud rate %d", baud); + return 0; + } + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= (CS8 | CLOCAL); + + if (serialFlow) + options.c_cflag |= CRTSCTS; // hardware flow control + else + options.c_cflag &= ~(CRTSCTS); // no hardware flow control + + // At 115k (87 us per char), reading 1 char at a time results in increased + // CPU usage, since we actually can keep up with getting a small number of + // characters per loop. At 921k (11 us per char), we get more characters + // each time through the loop, so there is less advantage to setting VMIN + // to more than 1. + // + // CPU Usage at + // VMIN 115k 921k + // 1 7.0% 1.8% + // 10 2.7% 1.6% + // 100 1.2% 1.2% + // + // The problem with asking for more than 1 character per read is that each + // message will usually not be received until some bytes in the following + // message are available. That is often not a problem, but there are + // sometimes gaps of several 10s of milliseconds in the telemetry stream, + // and it is preferable to process messages as soon as they are available. + if (baud <= 115200) + options.c_cc[VMIN] = 10; + else + options.c_cc[VMIN] = 1; + options.c_cc[VTIME] = 0; + + tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW" + + syslog(LOG_INFO, "opened serial port %s", serialPortName.c_str()); + + return 1; +} + +void set_seen_sysid_compid_from_client(uint8_t target_system, uint8_t target_component, + sockdata *client) +{ + pthread_mutex_lock(&mutex_arp_table); + if (!arp_table[target_system][target_component]) { + syslog(LOG_INFO, "ARP: Seen (%d/%d) from %s:%d client=%p", target_system, target_component, + inet_ntoa(client->sa.sin_addr), ntohs(client->sa.sin_port), &client); + arp_table[target_system][target_component] = client; + arp_table[target_system][0] = client; + } + pthread_mutex_unlock(&mutex_arp_table); +} + +bool seen_sysid_locked(uint8_t target_system) +{ + bool ret = true; + if (arp_table.count(target_system) == 0) { + ret = false; + } + if (ret && arp_table[target_system].count(0) == 0) { + ret = false; + } + return ret; +} +bool seen_sysid(uint8_t target_system) +{ + bool ret; + pthread_mutex_lock(&mutex_arp_table); + ret = seen_sysid_locked(target_system); + pthread_mutex_unlock(&mutex_arp_table); + return ret; +} + +bool seen_sysid_compid(uint8_t target_system, uint8_t target_component) +{ + bool ret = true; + pthread_mutex_lock(&mutex_arp_table); + if (!seen_sysid_locked(target_system)) { + ret = false; + } + if (ret && !arp_table[target_system].count(target_component)) { + ret = false; + } + pthread_mutex_unlock(&mutex_arp_table); + return ret; +} + +bool clients_match(const sockdata *a, const sockdata *b) +{ + if (a == NULL || b == NULL) { + // should never get here + return false; + } + return (a->sa.sin_port == b->sa.sin_port && a->sa.sin_addr.s_addr == b->sa.sin_addr.s_addr); +} + +void purge_client_from_arp_table(sockdata *client) +{ + pthread_mutex_lock(&mutex_arp_table); + for (std::map< uint8_t, std::map< uint8_t, sockdata * > >::iterator it = arp_table.begin(); + it != arp_table.end(); it++) { + std::map< uint8_t, sockdata * >::iterator next = (*it).second.begin(); + for (std::map< uint8_t, sockdata * >::iterator iv = next; iv != (*it).second.end(); + iv = next) { + next++; + const sockdata *some_client = (*iv).second; + if (clients_match(some_client, client)) { + const uint8_t target_system = (*it).first; + const uint8_t target_component = (*iv).first; + syslog(LOG_INFO, "ARP: Purging (%d/%d) from %s:%d client=%p", target_system, + target_component, inet_ntoa(client->sa.sin_addr), ntohs(client->sa.sin_port), + &client); + (*it).second.erase(target_component); + } + } + } + pthread_mutex_unlock(&mutex_arp_table); +} + +struct sockdata *find_client(uint64_t now_us, struct sockaddr_in remaddr) +{ + // If this is a new client, add them to the send list + // Maybe better to do this with find_if() and a search function but... + // If we do find a match, update its latest received time + + for (list< sockdata * >::iterator it = clients.begin(); it != clients.end(); ++it) { + if ((*it)->sa.sin_addr.s_addr == remaddr.sin_addr.s_addr && + (*it)->sa.sin_port == remaddr.sin_port) { + return (*it); + } + } + + // Skip the artoo address + if ((remaddr.sin_addr.s_addr == artooaddr.sin_addr.s_addr && + remaddr.sin_port == artooaddr.sin_port)) { + return NULL; + } + syslog(LOG_INFO, "adding new client %s:%d", inet_ntoa(remaddr.sin_addr), + ntohs(remaddr.sin_port)); + + struct sockdata *client = new sockdata(); + client->sa = remaddr; + + pthread_mutex_lock(&mutex_clients); + clients.push_back(client); + pthread_mutex_unlock(&mutex_clients); + + return client; +} + +/*********************************************************************** +Function: void *upstream_task(void*) + +Description: The upstream thread task. Checks for data available on the + UDP and puts it in the serial port. + + Maintains client list: all "clients" receive the telemetry + stream from Pixhawk. A client is added to the list by + sending any telemetry upstream (to the UDP port). A client + is removed from the list if it has not sent anything + upstream for a while. +***********************************************************************/ +void check_client_timeouts(uint64_t now_us); +void *upstream_task(void *) +{ + int recvlen; + struct sockaddr_in remaddr; + socklen_t addrlen = sizeof(remaddr); + unsigned char buf[BUFSIZE]; + list< sockdata >::iterator it, next; + uint64_t now_us; + int txBufUsage; + mavlink_message_t mav_msg; + mavlink_status_t mav_status; + mavlink_command_long_t mav_cmd_long; + int i; + bool discard; + + uint64_t next_client_timeout_check = 0; + uint32_t client_timeout_check_interval = 5000000; // every 5 seconds + + while (true) { + // Attempt to receive data, this will timeout + recvlen = recvfrom(sock_fd, buf, BUFSIZE, 0, (struct sockaddr *)&remaddr, &addrlen); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + if (recvlen < 0) { + if (errno != EAGAIN) + syslog(LOG_ERR, "upstream socket error: %s", strerror(errno)); + + if (next_client_timeout_check < now_us) { + check_client_timeouts(now_us); + next_client_timeout_check = now_us + client_timeout_check_interval; + } + + continue; + } + + struct sockdata *client = find_client(now_us, remaddr); + + if (recvlen > 0) { + + // check that datagram can be a valid mavlink message + if (recvlen < 8) { + // too short - need enough for 6-byte header and 2-byte crc + pthread_mutex_lock(&mutex_upStats); + upBadShort++; + pthread_mutex_unlock(&mutex_upStats); + } else if (buf[0] != 254) { + // bad magic - first byte should always be 254 + pthread_mutex_lock(&mutex_upStats); + upBadMagic++; + pthread_mutex_unlock(&mutex_upStats); + } else if (recvlen != (buf[1] + 8)) { + // bad length - total packet length is header(6) + len(buf[1]) + crc(2) + pthread_mutex_lock(&mutex_upStats); + upBadLength++; + pthread_mutex_unlock(&mutex_upStats); + } else { + // sanity checked - might be a good mavlink message + + pthread_mutex_lock(&mutex_upStats); + upMsgs++; + upBytes += recvlen; + pthread_mutex_unlock(&mutex_upStats); + +#if 1 // debug + + if (buf[5] == MAVLINK_MSG_ID_SET_MODE) { + syslog(LOG_INFO, "SET_MODE: from %s:%d", inet_ntoa(remaddr.sin_addr), + ntohs(remaddr.sin_port)); + // we don't have a mav_msg to use for unpacking + // custom_mode is the first four bytes, little endian + syslog(LOG_INFO, "SET_MODE: len=%u src_sys=%u src_comp=%u", unsigned(buf[1]), + unsigned(buf[3]), unsigned(buf[4])); + if (unsigned(buf[1]) == MAVLINK_MSG_ID_SET_MODE_LEN) + syslog(LOG_INFO, + "SET_MODE: custom_mode=%u.%u.%u.%u target_system=%u base_mode=%u", + unsigned(buf[9]), unsigned(buf[8]), unsigned(buf[7]), + unsigned(buf[6]), unsigned(buf[10]), unsigned(buf[11])); + } + +#endif // debug + + // Check the amount of data in the tx buffer. + ioctl(serial_fd, TIOCOUTQ, &txBufUsage); + + // At 921k baud, the UART send buffer holds about 44 msec of + // data. A burst of uplink can lead to messages being dropped. + // Since the uplink thread only relays messages from the UDP + // port to the serial port, the possible fixes include: (a) + // don't read from UDP until we know there's room in serial; + // (b) read and buffer messages in the thread until there is + // room in serial; (c) wait here until there is room. (c) is + // chosen because it is very simple, and is equivalent to (a) + // but with less logic. The only advantage of (b) would be a + // bit more flexibility in how messages are buffered; here, we + // are just leaving them in the UDP socket buffer. A still- + // smarter option would be to handle wifi uplink and local + // uplink differently (we can't tell the difference right + // now), or to even choose messages to drop in the case of + // congestion. + + if ((4000 - txBufUsage) < recvlen) { + // Congestion; serial port tx buffer is filling up + syslog(LOG_INFO, "serial port tx full; waiting for room"); + + // Worst-case wait time is if we have a long message to + // send and are running at a low baud rate. For a long + // 263-byte message and a low baud rate of 57600, that's + // ~50 msec. A typical wait time should be ~50 bytes at + // 921k baud, or ~500 usec. Allow for Pixhawk busy and not + // pulling messages. Reaching this timeout is a fatal + // error and means something is broken; err on the side of + // no false detections yet recovering (restarting this + // process) 'quicky' in human terms. + const unsigned serial_timeout_us = 1000000; // 1 sec + uint64_t start_us = clock_gettime_us(CLOCK_MONOTONIC); + while (true) { + uint64_t poll_us = clock_gettime_us(CLOCK_MONOTONIC); + ioctl(serial_fd, TIOCOUTQ, &txBufUsage); + if ((4000 - txBufUsage) >= recvlen) + break; + unsigned wait_us = poll_us - start_us; + if (wait_us > serial_timeout_us) { + // Either imx6 output serial port is stuck, or + // Pixhawk is not reading from its end. + // Restarting the process may fix the former. + syslog(LOG_ERR, "serial port tx stuck; exiting"); + exit(1); + } else { + // There's close to 4000 bytes queued in the port, + // or over 40 msec even at 921k baud. Let some of + // it go. + usleep(10000); // 10 msec + } + } + syslog(LOG_INFO, "serial port tx was full; now resuming"); + } + + // If we're rc-locked, don't send an arm command + // Check by decoding mavlink COMMAND_LONG packets when the RC is locked. + + discard = false; + + if (rc_locked) { + if (buf[5] == MAVLINK_MSG_ID_COMMAND_LONG) { + for (i = 0; i < recvlen; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &mav_msg, &mav_status)) { + mavlink_msg_command_long_decode(&mav_msg, &mav_cmd_long); + if (mav_cmd_long.command == MAV_CMD_COMPONENT_ARM_DISARM && + mav_cmd_long.param1 == 1) { + syslog(LOG_ERR, + "arm command received but RC is locked out; ignoring"); + discard = true; + } + } + } + } + } + + if (!discard) { + if (write(serial_fd, buf, recvlen) != recvlen) { + int err = errno; + syslog(LOG_ERR, "serial port write error: %s", strerror(err)); + } else { + pthread_mutex_lock(&mutex_upStats); + serialUpBytes += recvlen; + pthread_mutex_unlock(&mutex_upStats); + } + } + + if (client != NULL) { + set_seen_sysid_compid_from_client(buf[3], buf[4], client); + } + } // sanity checked as possibly good mavlink message + + // Client checking is done outside the sanity-checked mavlink + // message clause so we can send a byte to the port as a way of + // registering to receive downstream messages. Debt incurred. + if (client != NULL) { + client->lastRecvTime_us = now_us; + } + + if (next_client_timeout_check < now_us) { + check_client_timeouts(now_us); + next_client_timeout_check = now_us + client_timeout_check_interval; + } + + } // if (recvlen > 0) + + } // while (true) + + pthread_exit(NULL); + +} // upstream_task + +void check_client_timeouts(uint64_t now_us) +{ + list< sockdata * >::iterator it, next; + + // Random GCS that connect to telem_forward - + // e.g. dataflash_logger - might suddenly go away and come + // back again. + // Check if any client has timed out. This is done on any message or + // socket timeout - if done only on socket timeout, it might never be + // done. + pthread_mutex_lock(&mutex_clients); + for (it = clients.begin(); it != clients.end(); it = next) { + next = it; + next++; + + struct sockdata *client = *it; + uint64_t delta_us = now_us - client->lastRecvTime_us; + + if (delta_us > CLIENT_TIMEOUT_US) { + syslog(LOG_INFO, "client %s:%d timed out after %0.3fs of inactivity; removing", + inet_ntoa(client->sa.sin_addr), ntohs(client->sa.sin_port), delta_us / 1.0e6); + purge_client_from_arp_table(client); + clients.erase(it); + delete client; + } + } + pthread_mutex_unlock(&mutex_clients); +} + +/********************************************************************** +Function: int start_upstream_thread() + +Description: Starts the upstream thread. + ***********************************************************************/ +int start_upstream_thread(void) +{ + int ret = 0; + struct sched_param param; + + // Start the upstream thread + pthread_create(&upstream_ctx, NULL, upstream_task, NULL); + + pthread_setname_np(upstream_ctx, "telem_up"); + + // upstream priority and schedule + param.sched_priority = UPSTREAM_PRIORITY; + if ((ret = pthread_setschedparam(upstream_ctx, SCHED_FIFO, ¶m))) + syslog(LOG_ERR, "could not set upstream schedule priority: %d", ret); + + return ret; +} + +/********************************************************************** +Function: bool handle_system_time(const mavlink_message_t& msg) + +Description: Handle a mavlink SYSTEM_TIME message. This message + contains the GPS time (some "GPS" messages do not). The + GPS time is always logged, and if enabled, the system + time is set. +***********************************************************************/ +static bool handle_system_time(const mavlink_message_t &msg) +{ + mavlink_system_time_t system_time; + + mavlink_msg_system_time_decode(&msg, &system_time); + + // GPS time is good if it is nonzero + if (system_time.time_unix_usec != 0) { + struct tm utc; + char utc_buf[80]; + struct timeval tv; + + tv.tv_sec = system_time.time_unix_usec / 1000000ULL; + tv.tv_usec = system_time.time_unix_usec % 1000000ULL; + + gmtime_r(&tv.tv_sec, &utc); + + strftime(utc_buf, sizeof(utc_buf), "%F %T", &utc); + + syslog(LOG_INFO, "GPS time: %s.%06ld", utc_buf, tv.tv_usec); + + if (useGpsTime) { + int status = settimeofday(&tv, NULL); + if (status != 0) + syslog(LOG_ERR, "setting clock from GPS"); + else + syslog(LOG_INFO, "clock set from GPS"); + } + return true; + } + + return false; + +} // handle_system_time + +bool get_route_targets(uint8_t &target_system, uint8_t &target_component, mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: + target_system = mavlink_msg_remote_log_data_block_get_target_system(&msg); + target_component = mavlink_msg_remote_log_data_block_get_target_component(&msg); + break; + case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: + target_system = mavlink_msg_remote_log_block_status_get_target_system(&msg); + target_component = mavlink_msg_remote_log_block_status_get_target_component(&msg); + break; + default: + return false; + } + + return true; +} + +bool should_send_message_to_client(sockdata &client, mavlink_message_t &msg) +{ + // return true; + + uint8_t target_system; + uint8_t target_component; + if (!get_route_targets(target_system, target_component, msg)) { + // no targets in packet - must route + return true; + } + + if (target_system == 0) { + // broadcast + return true; + } + + if (!seen_sysid(target_system)) { + return false; + } + + if (target_component == 0) { + // broadcast to specific system + return true; + } + + if (!seen_sysid_compid(target_system, target_component)) { + return false; + } + + return clients_match(arp_table[target_system][target_component], &client); +} + +/********************************************************************** +Function: void *downstream_task(void*) + +Description: The downstream thread task. Waits for data on either the + serial port or inject port and outputs it to all connected + clients on the UDP port. The inject port is a way to + inject telemetry into the down-bound stream. +***********************************************************************/ +bool artoo_should_get_msg(const mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: + case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: + return false; + default: + return true; + } + return true; +} + +void *downstream_task(void *) +{ + int readlen; + char buf[BUFSIZE]; + list< sockdata * >::iterator it; + mavlink_message_t msg; + mavlink_status_t mavlinkStatus; + int i; + LinkPacket packet; + uint32_t link_next_seq = 0; + int messageLen; + const int fdn_serial = 0; // entry in fds[] used for serial + const int fdn_inject = 1; // entry in fds[] used for inject + const int nfds = 2; // entries in fds[] + struct pollfd fds[nfds]; + int pollrc; + int poll_errno_last = 0; + bool poll_zero = false; + int recv_errno_last = 0; + bool recv_zero = false; + uint8_t *p_packet_payload = packet.payload; // Pointer to the current payload addr + int payload_used = 0; // The amount of data currently in the payload + uint8_t mav_msg_buf[MAVLINK_MAX_PACKET_LEN]; // Temporary place to store the mavlink message + + /* Downlink logging */ + static uint64_t dl_err_logged_us = 0; + static unsigned dl_err_interval_us = 1000000; + static unsigned dl_err_count = 0; + + memset(&fds, 0, sizeof(fds)); + + fds[fdn_serial].fd = serial_fd; + fds[fdn_serial].events = POLLIN; + + fds[fdn_inject].fd = inject_fd; + fds[fdn_inject].events = POLLIN; + + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + uint64_t next_send_us = now_us + SEND_DT_US; + + while (true) { + + uint64_t pre_poll_us = clock_gettime_us(CLOCK_MONOTONIC); + + // Wait for available data. The timeout is based on the amount + // of time until the next transmit time, as we don't want to block + // any longer than that in order to keep sending every SEND_DT_US. + int timeout = (next_send_us - pre_poll_us) / 1000; + pollrc = poll(fds, nfds, (timeout < 0 ? 0 : timeout)); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + uint32_t blocked_us = now_us - pre_poll_us; + + if (pollrc < 0) { + // print error message only when errno changes to avoid log storms + if (errno != poll_errno_last) { + int err = errno; + syslog(LOG_ERR, "poll: %s", strerror(err)); + poll_errno_last = err; + } + } else if (pollrc == 0) { + // used for ppoll() timeout; should not get this from poll() + if (!poll_zero) { + syslog(LOG_ERR, "poll returned zero"); + poll_zero = true; + } + } else // (pollrc > 0) + { + + if (fds[fdn_serial].revents & POLLIN) { + // new serial data + readlen = read(serial_fd, buf, BUFSIZE); + if (readlen > 0) { + pthread_mutex_lock(&mutex_downStats); + serialDownBytes += readlen; + pthread_mutex_unlock(&mutex_downStats); + + for (i = 0; i < readlen; ++i) { + // Only send full mavlink messages at a time + // mavlink_frame_char is buffering up the message, + // so we don't have to keep buf[] data after giving + // it to mavlink_parse_char + uint8_t frame_check = + mavlink_frame_char(MAVLINK_COMM_0, buf[i], &msg, &mavlinkStatus); + /* + if frame_check == MAVLINK_FRAMING_BAD_CRC + then we got a message frame but with a bad + CRC. That may indicate a new message not in + our XML. We want to forward those, but not + process locally + */ + if (frame_check != MAVLINK_FRAMING_INCOMPLETE) { + if (frame_check == MAVLINK_FRAMING_OK) { + if (!gotGpsTime && msg.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) { + // Log GPS time, and optionally set system clock + gotGpsTime = handle_system_time(msg); + } + } + + // Check sequence if we've seen this (srcid, + // compid) before + pthread_mutex_lock(&mutex_streams); + list< stream_state >::iterator i; + bool found_stream = false; + // This is typically a list of one, maybe two + for (i = streams.begin(); i != streams.end(); i++) { + if (msg.sysid == i->sysid && msg.compid == i->compid) { + if (msg.seq != ((i->seq + 1) & 0xff)) + i->seq_err++; + i->seq = msg.seq; + found_stream = true; + break; + } + } + // Create stream state if necessary and message is + // known good. + if (!found_stream && frame_check == MAVLINK_FRAMING_OK) { + stream_state new_stream(msg.sysid, msg.compid, msg.seq); + streams.push_back(new_stream); + } + pthread_mutex_unlock(&mutex_streams); + + messageLen = mavlink_msg_to_send_buffer(mav_msg_buf, &msg); + + pthread_mutex_lock(&mutex_downStats); + downMsgs++; + downMsgsById[msg.msgid]++; + downBytes += messageLen; + pthread_mutex_unlock(&mutex_downStats); + + // Start with the artoo + if (artoo_should_get_msg(msg)) { + /* If there isn't enough room in the LinkPacket to store this + * message, + * send the current LinkPacket along and make this message the first + * in + * the next LinkPacket. Worst case, this message will get there in + * another + * 100ms */ + if (messageLen > (packet.MAX_PAYLOAD - payload_used)) { + /* Force a send */ + syslog(LOG_ERR, "Message too large for aggregate buffer, " + "sending LinkPacket first"); + packet.tf_send_us = clock_gettime_us(CLOCK_MONOTONIC); + packet.tc_recv_us = 0; + packet.tc_send_us = 0; + packet.stm_recv_us = 0; + packet.seq = link_next_seq++; + packet.data1 = payload_used; + packet.data2 = blocked_us; + packet.data3 = 0; + if (sendto(sock_fd, &packet, LinkPacket::HDR_LEN + payload_used, + 0, (struct sockaddr *)&artooaddr, + sizeof(artooaddr)) < 0) { + // rate-limited logging (dl = downlink) + dl_err_count++; + // log it if this is the first error, or if + // it has been long enough since logging an error + if (dl_err_logged_us == 0 || + (now_us - dl_err_logged_us) >= dl_err_interval_us) { + // log error + syslog(LOG_ERR, "sendto(downlink): %s (%u errors)", + strerror(errno), dl_err_count); + dl_err_logged_us = now_us; + dl_err_count = 0; + } + } + + /* Reset the payload info */ + payload_used = 0; + p_packet_payload = packet.payload; + } + + /* Put the packet into our packet buffer */ + memcpy(p_packet_payload, mav_msg_buf, messageLen); + p_packet_payload += messageLen; + + /* If this is the first packet, set the timestamp */ + if (!payload_used) + packet.tf_recv_us = now_us; + + payload_used += messageLen; + } + // Continue with any other connected clients + pthread_mutex_lock(&mutex_clients); + for (it = clients.begin(); it != clients.end(); ++it) { + if (!should_send_message_to_client(**it, msg)) { + continue; + } + + if (sendto(sock_fd, mav_msg_buf, messageLen, 0, + (struct sockaddr *)&((*it)->sa), + sizeof((*it)->sa)) < 0) { + // rate-limited logging (ll = local link) + static uint64_t ll_err_logged_us = 0; + static unsigned ll_err_interval_us = 1000000; + static unsigned ll_err_count = 0; + ll_err_count++; + // log it if this is the first error, or if + // it has been long enough since logging an error + if (ll_err_logged_us == 0 || + (now_us - ll_err_logged_us) >= ll_err_interval_us) { + // log error + syslog(LOG_ERR, "sendto(local link): %s (%u errors)", + strerror(errno), ll_err_count); + ll_err_logged_us = now_us; + ll_err_count = 0; + } + } + } + pthread_mutex_unlock(&mutex_clients); + } + } // for (i...) + } // if (readlen...) + } // if (fds[fdn_serial]...) + + if (fds[fdn_inject].revents & POLLIN) { + + // new inject data - should get one MAVLink message per recv + readlen = recv(inject_fd, mav_msg_buf, MAVLINK_MAX_PACKET_LEN, 0); + if (readlen < 0) { + // print error message only when errno changes to avoid + // log storms + if (errno != recv_errno_last) { + int err = errno; + syslog(LOG_ERR, "recv: %s", strerror(err)); + recv_errno_last = err; + } + } else if (readlen == 0) { + // this means the peer has performed an orderly shutdown + // which does not make sense for us + if (!recv_zero) { + syslog(LOG_ERR, "recv returned zero"); + recv_zero = true; + } + } else // readlen > 0 + { + /* Put the packet into our packet buffer */ + if (readlen <= (packet.MAX_PAYLOAD - payload_used)) { + memcpy(p_packet_payload, mav_msg_buf, readlen); + p_packet_payload += readlen; + + /* If this is the first packet, set the timestamp */ + if (!payload_used) + packet.tf_recv_us = now_us; + + payload_used += readlen; + } else { + /* Whoops, we dropped one here */ + syslog(LOG_ERR, "Inject message too large for aggregate buffer, dropping"); + } + + pthread_mutex_lock(&mutex_downStats); + downBytes += readlen; + // downMsgsById[msg.msgid]++; // XXX could pluck out mavlink msg id + downMsgs++; + pthread_mutex_unlock(&mutex_downStats); + + // continue with any other connected clients + pthread_mutex_lock(&mutex_clients); + for (it = clients.begin(); it != clients.end(); ++it) { + if (sendto(sock_fd, mav_msg_buf, readlen, 0, + (struct sockaddr *)&((*it)->sa), sizeof((*it)->sa)) < 0) { + // XXX + } + } + pthread_mutex_unlock(&mutex_clients); + + } // if (readlen...) + + } // if (fds[fdn_serial]...) + + } // if (pollrc...) + + /* Determine if it is time to send data to Artoo. Either its been enough + * time or we don't have enough room to store the largest mavlink message (263B). + * Also make sure we actually have something to send. Always send if locked, + * since we might be locked because we are running with an old controller that + * does not understand aggregation (and we don't care about link efficiency). */ + if (rc_locked || (now_us > next_send_us) || + (payload_used >= (packet.MAX_PAYLOAD - MAVLINK_MAX_PACKET_LEN))) { + + if (payload_used > 0) { + /* packet.tf_recv_us will have already been set by the first packet */ + packet.tf_send_us = clock_gettime_us(CLOCK_MONOTONIC); + packet.tc_recv_us = 0; + packet.tc_send_us = 0; + packet.stm_recv_us = 0; + packet.seq = link_next_seq++; + packet.data1 = payload_used; + packet.data2 = blocked_us; + packet.data3 = 0; + if (sendto(sock_fd, &packet, LinkPacket::HDR_LEN + payload_used, 0, + (struct sockaddr *)&artooaddr, sizeof(artooaddr)) < 0) { + // rate-limited logging (dl = downlink) + dl_err_count++; + // log it if this is the first error, or if + // it has been long enough since logging an error + if (dl_err_logged_us == 0 || + (now_us - dl_err_logged_us) >= dl_err_interval_us) { + // log error + syslog(LOG_ERR, "sendto(downlink): %s (%u errors)", strerror(errno), + dl_err_count); + dl_err_logged_us = now_us; + dl_err_count = 0; + } + } + + /* Reset the payload info */ + payload_used = 0; + p_packet_payload = packet.payload; + } + + /* Set the next time we should send */ + next_send_us = now_us + SEND_DT_US; + } + + } // while (true) + + pthread_exit(NULL); +} + +/********************************************************************** +Function: int start_downstream_thread() + +Description: Starts the downstream thread. + ***********************************************************************/ +int start_downstream_thread(void) +{ + int ret = 0; + struct sched_param param; + + // Start the downstream thread + pthread_create(&downstream_ctx, NULL, downstream_task, NULL); + + pthread_setname_np(downstream_ctx, "telem_down"); + + // downstream priority and schedule + param.sched_priority = DOWNSTREAM_PRIORITY; + if ((ret = pthread_setschedparam(downstream_ctx, SCHED_FIFO, ¶m))) + syslog(LOG_ERR, "could not set downstream schedule priority: %d", ret); + + return ret; +} + +/********************************************************************** +Function: void *logging_task(void*) + +Description: The logging task. Dumps logging info to our log file + periodically. +***********************************************************************/ +void *logging_task(void *) +{ + uint64_t delta_us; + uint64_t now_us, last_log_us; + + last_log_us = clock_gettime_us(CLOCK_MONOTONIC); + + while (1) { + // Log the amount of downstream data + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + delta_us = now_us - last_log_us; + + if (delta_us > LOG_DT_US) { + last_log_us = now_us; + + pthread_mutex_lock(&mutex_downStats); + pthread_mutex_lock(&mutex_upStats); + + // utilization of serial link + + // bytes/sec used = serialBytes / (delta_us / 1e6) + // bytes/sec capacity = serialBaud / 10 + // percent utilization = [serialBytes / (delta_us / 1e6)] / (serialBaud / 10) * 100% + // = (serialBytes * 1e7) / (delta_us * serialBaud) * 100% + + double downPct = (serialDownBytes * 1e9) / ((double)(delta_us * serialBaud)); + double upPct = (serialUpBytes * 1e9) / ((double)(delta_us * serialBaud)); + + syslog(LOG_INFO, "down: %d m/s, %d b/s, %d%%; up: %d m/s, %d b/s, %d%%", + int(downMsgs * 1e6 / delta_us), int(downBytes * 1e6 / delta_us), int(downPct), + int(upMsgs * 1e6 / delta_us), int(upBytes * 1e6 / delta_us), int(upPct)); + if (upBadShort != 0 || upBadMagic != 0 || upBadLength != 0) { + syslog(LOG_INFO, "up err: %u, %u, %u", upBadShort, upBadMagic, upBadLength); + upBadShort = 0; + upBadMagic = 0; + upBadLength = 0; + } + + downMsgs = 0; + downBytes = 0; + upMsgs = 0; + upBytes = 0; + serialDownBytes = 0; + serialUpBytes = 0; + + if (logControl & LOG_CONTROL_MSG_COUNTS_DOWN) { + ostringstream dbg; + dbg << "by id:"; + for (int id = ID_MIN; id < ID_MAX; id++) { + if (downMsgsById[id] > 0) { + // 77:108 means there were 108 messages with id 77 + dbg << ' ' << id << ':' << downMsgsById[id]; + downMsgsById[id] = 0; + } + } + syslog(LOG_INFO, "%s", dbg.str().c_str()); + } + + pthread_mutex_unlock(&mutex_upStats); + pthread_mutex_unlock(&mutex_downStats); + + // sequence errors + bool any_errs = false; + list< stream_state >::iterator i; + ostringstream dbg; + dbg << "seq errs:"; + pthread_mutex_lock(&mutex_streams); + for (i = streams.begin(); i != streams.end(); i++) { + // ignore gimbal (sequence is bogus) + if ((i->sysid != 1 || i->compid != 154) && i->seq_err > 0) { + // cast to int is so << doesn't print a char + dbg << " (" << int(i->sysid) << "," << int(i->compid) << "):" << i->seq_err; + i->seq_err = 0; + any_errs = true; + } + } + pthread_mutex_unlock(&mutex_streams); + if (any_errs) { + syslog(LOG_INFO, "%s", dbg.str().c_str()); + } + } + + // Put a big sleep here + sleep(1); // 1s + } + pthread_exit(NULL); +} + +/********************************************************************** +Function: int start_logging_thread() + +Description: Starts the logging thread. + ***********************************************************************/ +int start_logging_thread(void) +{ + int ret = 0; + struct sched_param param; + + // Start the logging thread + pthread_create(&logging_ctx, NULL, logging_task, NULL); + + pthread_setname_np(logging_ctx, "telem_log"); + + // logging priority and schedule + param.sched_priority = LOG_PRIORITY; + if ((ret = pthread_setschedparam(logging_ctx, SCHED_FIFO, ¶m))) + syslog(LOG_ERR, "could not set logging schedule priority: %d", ret); + + return ret; +} + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(void) +{ + int destPort; + string inject_name; + + openlog("tlm", LOG_NDELAY, LOG_LOCAL4); + + syslog(LOG_INFO, "Installing SIGQUIT signal handler"); + signal(SIGQUIT, sigquit_handler); + + serialDownBytes = 0; + serialUpBytes = 0; + + syslog(LOG_INFO, "telem_forwarder starting: built " __DATE__ " " __TIME__); + + /* Parse the sololink.conf file for serial port, source IPs and ports */ + INIReader reader("/etc/sololink.conf"); + + if (reader.ParseError() < 0) { + syslog(LOG_CRIT, "can't parse /etc/sololink.conf"); + return -1; + } + + /* Get the serial port to output DSM data on */ + serialPortName = reader.Get("solo", "telemDev", "/dev/ttymxc1"); + serialBaud = reader.GetInteger("solo", "telemBaud", 57600); + serialFlow = reader.GetBoolean("solo", "telemFlow", true); + inject_name = reader.Get("solo", "injectPortName", "/run/telem_downlink"); + + /* Get the udp destination port for clients */ + destPort = reader.GetInteger("solo", "telemDestPort", 14550); + + /* Whether to set system time from GPS or not */ + useGpsTime = reader.GetBoolean("solo", "useGpsTime", true); + + /* Downstream telemetry TOS */ + int tos = reader.GetInteger("solo", "telemDownTos", 0x7f); + + /* Logging control (debug) */ + logControl = reader.GetInteger("solo", "telemLogControl", 0); + + syslog(LOG_INFO, "serial port: %s", serialPortName.c_str()); + syslog(LOG_INFO, "serial baudrate: %d", serialBaud); + syslog(LOG_INFO, "serial hw flow: %s", serialFlow ? "true" : "false"); + syslog(LOG_INFO, "UDP dest port: %d", destPort); + syslog(LOG_INFO, "use GPS time: %s", useGpsTime ? "true" : "false"); + syslog(LOG_INFO, "tos: 0x%02x", (uint8_t)tos); + if (logControl != 0) + syslog(LOG_INFO, "log control: 0x%08x", logControl); + + /* Set up the various ports */ + + if (!UDP_setup(tos)) { + syslog(LOG_CRIT, "unable to initialize the UDP receive"); + return -1; + } + + if (!serial_setup(serialBaud)) { + syslog(LOG_CRIT, "unable to initialize the serial send"); + return -1; + } + + if (!inject_setup(inject_name.c_str())) { + syslog(LOG_CRIT, "unable to initialize the inject port"); + return -1; + } + + // Set up the mutexes + pthread_mutex_init(&mutex_upStats, NULL); + pthread_mutex_init(&mutex_downStats, NULL); + pthread_mutex_init(&mutex_clients, NULL); + pthread_mutex_init(&mutex_streams, NULL); + + // Setup the Artoo address and port + memset((char *)&artooaddr, 0, sizeof(artooaddr)); + artooaddr.sin_family = AF_INET; + inet_aton("10.1.1.1", &artooaddr.sin_addr); + artooaddr.sin_port = htons(destPort); + + rc_locked = RcLock::locked(); + + /* Start the upstream, downstream, and logging threads */ + start_downstream_thread(); + start_upstream_thread(); + start_logging_thread(); + + // The main while() loop just becomes a thread monitor. If any thread dies, + // exit and let inittab restart us + while (true) { + rc_locked = RcLock::locked(); + + if (pthread_kill(upstream_ctx, 0) != 0 || pthread_kill(downstream_ctx, 0) != 0 || + pthread_kill(logging_ctx, 0) != 0) { + syslog(LOG_ERR, "a thread terminated; exiting"); + exit(0); + } + + // Long sleep + sleep(1); + } + + return -1; +} diff --git a/flightcode/telem/test_telem.cpp b/flightcode/telem/test_telem.cpp new file mode 100644 index 0000000..8e8ba26 --- /dev/null +++ b/flightcode/telem/test_telem.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +/*********************************************************************** +UDP macros +***********************************************************************/ +#define REMOTE_UDP_PORT 14560 +#define LOCAL_UDP_PORT 14550 +#define NUM_CHANNELS 8 + +int main(void) +{ + int sock_fd; + struct sockaddr_in myaddr; + struct sockaddr_in remaddr; /* server address */ + int slen = sizeof(remaddr); + socklen_t addrlen = sizeof(remaddr); /* length of addresses */ + char *server = "127.0.0.1"; /* change this to use a different server */ + char buf[512]; + int recvlen; + + if ((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + cerr << "cannot create socket" << endl; + return 0; + } + + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(LOCAL_UDP_PORT); + + if (bind(sock_fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + perror("bind failed"); + return 0; + } + + fcntl(sock_fd, F_SETFL, O_NONBLOCK); + + memset((char *)&remaddr, 0, sizeof(remaddr)); + remaddr.sin_family = AF_INET; + remaddr.sin_port = htons(REMOTE_UDP_PORT); + if (inet_aton(server, &remaddr.sin_addr) == 0) { + cerr << "inet_aton() failed" << endl; + return 0; + } + + memset(buf, 0, sizeof(buf)); + sprintf(buf, "Hello!\n"); + sendto(sock_fd, buf, strlen(buf), 0, (struct sockaddr *)&remaddr, slen); + + while (true) { + // See if theres anything to receive + memset(buf, 0, sizeof(buf)); + recvlen = recvfrom(sock_fd, buf, 512, 0, (struct sockaddr *)&remaddr, &addrlen); + + if (recvlen > 0) + cout << "Got " << recvlen << " bytes" << endl; + } + + close(sock_fd); + return 1; +} diff --git a/flightcode/telem_ctrl/Makefile b/flightcode/telem_ctrl/Makefile new file mode 100644 index 0000000..36255fe --- /dev/null +++ b/flightcode/telem_ctrl/Makefile @@ -0,0 +1,48 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../ini ../ini/cpp + +INCS = -I../util -I../ini -I../ini/cpp + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS = -lpthread -lrt + +SRCS_CPP = telem_ctrl.cpp +SRCS_CPP += INIReader.cpp +SRCS_CPP += telem_dest.cpp +SRCS_C = arp_table.c hostapd_ctrl.c util.c ini.c +SRCS_C += mutex.c +SRCS_C += syslog.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = telem_ctrl + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/telem_ctrl/telem_ctrl.cpp b/flightcode/telem_ctrl/telem_ctrl.cpp new file mode 100644 index 0000000..fadc351 --- /dev/null +++ b/flightcode/telem_ctrl/telem_ctrl.cpp @@ -0,0 +1,933 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "INIReader.h" +#include "arp_table.h" +#include "hostapd_ctrl.h" +#include "telem_dest.h" +#include "util.h" +#include "link_packet.h" +#include "mutex.h" +#include "net_wmm.h" + +/* +* telem_ctrl +* +* Receive telemetry (mavlink) via UDP from Solo, and forward to: +* stm32 process (sends to controller's STM32) +* tlog process (creates tlog file on controller) +* any host detected on network that +* - is associated with AP (e.g. app, gcs) +* - appears in ARP table (e.g gcs running in VM) +* +* Intercept SYSTEM_TIME messages in downlink, and set system time from GPS the +* first time it is found valid. +* +* Receive telemetry in the upstream direction and forward to Solo. +* +* Log telemetry statistics periodically: +* downlink, total per (system id, component id) +* downlink, sequence errors per (system id, component id) +* uplink, total messages only +* +* This consumes about 2% of the CPU when forwarding 90 packets/second. +*/ + +/* process priority - this is used with SCHED_FIFO. A higher number is a + higher priority. When priorities are viewed on the target (in /proc, or + using lsproc.py), the number seen there will be (1 - number_here), so + e.g. if PRIORITY is 50, it will show up as priority -51. */ +#define TC_MAIN_PRIORITY 58 + +/* instead of pulling in all the mavlink headers...*/ +#define MAVLINK_MSG_ID_SYSTEM_TIME 2 + +/* controller sends mavlink to this port on all discovered telemetry destinations (gcs, app) */ +static short telem_dest_port_hbo; /* read from config on startup */ + +static pthread_t tlm_ctx; + +/* log stats this often */ +static uint64_t log_interval_us = 10 * 1000000; + +/* read from config; whether we set the system time when a GPS message + comes through */ +static int use_gps_time = true; + +/* becomes true when we get a good GPS time and set the system time */ +static int got_gps_time = 0; + +/* TOS used for telemetry packets from controller to solo */ +static int uplink_tos = IP_TOS_BE; + +/* TOS used for telemetry packets from controller to any GCS (including app) */ +static int gcs_tos = IP_TOS_BE; + +/* This is used to prevent swamping the log with error messages if + something unexpected happens. + Returns -1 if we cannot log an error now, or returns the number of + messages skipped due to rate limiting if we can, i.e. a return of + 2 means log, and we have skipped 2 messages due to rate limiting. */ +static int can_log_error(uint64_t now_us) +{ + static uint64_t err_time_us = 0; /* last time we logged */ + static uint64_t err_interval_us = 1000000; /* once per second max */ + static unsigned skipped = 0; + unsigned ret_val; + if ((now_us - err_time_us) < err_interval_us) { + /* can't log */ + skipped++; + return -1; + } + /* yes; say we can and set err_time_us assuming we do log something */ + err_time_us = now_us; + ret_val = skipped; + skipped = 0; + return ret_val; +} + +/* another process creates this file and writes Solo's IP to it when it connects */ +static const char *solo_ip_file = "/var/run/solo.ip"; + +/* how often to check it when waiting for Solo */ +static unsigned solo_ip_poll_us = 100000; + +/* solo's address */ +static struct sockaddr_in sa_solo; + +/* +* wait_solo - wait for Solo to appear on network +* +* Wait for the solo ip file to appear, and when it does, set our global solo +* ip address from it. This is called once at startup and does not return +* until Solo is there. +*/ +static void wait_solo(in_addr_t *solo_addr) +{ + FILE *fp; + char buf[40]; + char *status; + int err; + + while (1) { + + while ((fp = fopen(solo_ip_file, "r")) == NULL) + usleep(solo_ip_poll_us); + + memset(buf, 0, sizeof(buf)); + status = fgets(buf, sizeof(buf) - 1, fp); + + err = errno; /* save in case of error */ + + fclose(fp); + + if (status == buf) { + *solo_addr = inet_addr(buf); + if (*solo_addr != htonl(INADDR_NONE)) + return; + syslog(LOG_ERR, "converting \"%s\" to ip address", buf); + } else { + syslog(LOG_ERR, "reading %s: %s", solo_ip_file, strerror(err)); + } + /* loop around and try again, probably futile */ + sleep(1); + } + +} /* wait_solo */ + +/* +* telemetry destinations table +*/ + +/* how often to update the table */ +static unsigned telem_dest_update_interval_s = 5; + +/* instance of telemetry destinations table */ +static telem_dest_table dest_table; +static pthread_mutex_t dest_table_mutex; + +/* arp table, filled in by arp_table_get() each time we update the telemetry + destinations table */ +#define ARP_ENTRIES_MAX 32 +static arp_entry_t arp_table[ARP_ENTRIES_MAX]; + +/* table of everyone associated with the AP, filled in by each time we update + the telemetry destinations table */ +#define HOSTAPD_ENTRIES_MAX 32 +static hostapd_station_info_t hostapd_table[HOSTAPD_ENTRIES_MAX]; + +/* +* update_telem_dest - update the telemetry destinations table +* +* Add a destination when we have the ip (from arp). +* Remove a destination when the mac is not in hostapd's list and the ip is not +* in the arp table. +* +* In testing, it is now looking like IP addresses "never" disappear from the +* arp table; they are just marked with flags=0 when the entry times out (but +* the IP is still there). That means once an IP gets in the list of +* destinations, it will never be removed. For our purposes, that is okay. We +* could remove it when flags=0, but choose not to, with the fear that there +* may be some other condition that causes flags to be zero and we should not +* delete the destination. +*/ +static int update_telem_dest(void *hostapd_handle) +{ + int arp_entries; + int hostapd_entries; + int i; + + /* get arp table */ + memset(arp_table, 0, sizeof(arp_table)); + arp_entries = ARP_ENTRIES_MAX; + if (arp_table_get(arp_table, &arp_entries) != 0) { + syslog(LOG_ERR, "ERROR reading arp table"); + return -1; + } + + /* get associated stations table */ + memset(hostapd_table, 0, sizeof(hostapd_table)); + hostapd_entries = HOSTAPD_ENTRIES_MAX; + if (hostapd_ctrl_get_stations(hostapd_handle, hostapd_table, &hostapd_entries) != 0) { + syslog(LOG_ERR, "ERROR getting station info"); + return -1; + } + + /* The telemetry thread reads information from dest_table. To update it, + we copy it to work_table, update work_table, then copy work_table back + to dest_table inside a critical section. The telemetry thread might + hold the lock for a while as it sends a packet to all the destinations, + but this update function should hold it only for the final memcpy. */ + + telem_dest_table work_table; + memcpy(&work_table, &dest_table, sizeof(work_table)); + + /* for each entry in the arp table, if it is not in work_dest table, add it */ + arp_table_dump(LOG_DEBUG, arp_table, arp_entries); + for (i = 0; i < arp_entries; i++) { + int s = work_table.find_by_mac_ip(arp_table[i].mac, arp_table[i].ip); + /* is this arp entry one of our known telemetry destinations? */ + if (s == -1) { + struct in_addr in; + in.s_addr = arp_table[i].ip; + syslog(LOG_DEBUG, "arp entry %d is not a known telem dest", i); + if (in.s_addr == sa_solo.sin_addr.s_addr) { + syslog(LOG_DEBUG, "skipping telem dest at %s (solo)", inet_ntoa(in)); + } else if (arp_table[i].flags == 0) { + syslog(LOG_DEBUG, "skipping telem dest at %s (flags=0)", inet_ntoa(in)); + } else { + syslog(LOG_INFO, "adding telem dest @ %s:%d", inet_ntoa(in), telem_dest_port_hbo); + work_table.add(arp_table[i].mac, arp_table[i].ip, telem_dest_port_hbo, gcs_tos); + } + } else { + syslog(LOG_DEBUG, "arp entry %d is telem dest %d", i, s); + } + } + + /* for each telem dest, if its mac is not in hostapd's list and its ip is not in the arp table, + * remove it */ + syslog(LOG_DEBUG, "%d telemetry destinations", work_table.num_dest); + for (i = 0; i < work_table.num_dest; i++) { + /* don't mess with destinations on localhost */ + if (work_table.dest[i].sa.sin_addr.s_addr == htonl(INADDR_LOOPBACK)) { + syslog(LOG_DEBUG, "work_table.dest[%d] (%s:%d) is local", i, + inet_ntoa(work_table.dest[i].sa.sin_addr), + ntohs(work_table.dest[i].sa.sin_port)); + continue; + } + /* in hostapd's list? */ + if (hostapd_ctrl_find_by_mac(hostapd_table, hostapd_entries, work_table.dest[i].mac) != + -1) { + syslog(LOG_DEBUG, "work_table.dest[%d] (%s:%d) is associated", i, + inet_ntoa(work_table.dest[i].sa.sin_addr), + ntohs(work_table.dest[i].sa.sin_port)); + continue; + } + /* in arp table? */ + if (arp_table_find_by_ip(arp_table, arp_entries, work_table.dest[i].sa.sin_addr.s_addr) != + -1) { + syslog(LOG_DEBUG, "work_table.dest[%d] (%s:%d) is in the arp table", i, + inet_ntoa(work_table.dest[i].sa.sin_addr), + ntohs(work_table.dest[i].sa.sin_port)); + continue; + } + /* work_table.dest[i] is not associated with hostapd, and is not in arp table; + delete it */ + syslog(LOG_DEBUG, "work_table.dest[%d] is neither associated nor in arp table", i); + syslog(LOG_INFO, "deleting telem dest at %s", inet_ntoa(work_table.dest[i].sa.sin_addr)); + work_table.delete_by_index(i); + i--; /* the next entry is at [i] now */ + } + + mutex_lock(&dest_table_mutex, NULL); + + memcpy(&dest_table, &work_table, sizeof(dest_table)); + + mutex_unlock(&dest_table_mutex); + + dest_table.check(); + + return 0; + +} /* update_telem_dest */ + +/* +* sources[] table - one entry per mavlink source, containing expected next +* sequence number and basic stats (packet count, error count). +* +* Normally we only have two or three sources (pixhawk, rssi process, maybe +* gimbal). +* +* It has been observed in the past that we can get total garbage from Pixhawk +* (don't know if that can still happen). If that happens, the sources table +* will fill up with junk (bogus sys_id, comp_id), then we will stop adding +* new sources (they just won't be checked). Then the next time stats are +* printed (e.g. every 10 seconds), we'll print meaningless stats about the +* garbage, clear the table, and start over. If the junk goes away, this should +* recover. +*/ + +#define SOURCE_MAX 32 + +typedef struct { + uint8_t sys_id; + uint8_t comp_id; + uint8_t seq; + unsigned pkt_cnt; + unsigned err_cnt; +} source_info_t; + +static source_info_t sources[SOURCE_MAX]; +static int num_sources = 0; + +static void source_set(source_info_t *si, uint8_t sys_id, uint8_t comp_id, uint8_t seq) +{ + si->sys_id = sys_id; + si->comp_id = comp_id; + si->seq = seq; + si->pkt_cnt = 1; + si->err_cnt = 0; +} + +/* +* source_check - check sequence number of a message +* +* Also maintain basic statistics on messages from each source (packet count, +* error count). +* +* Return zero if sequence correct, nonzero if sequence error +*/ +static int source_check(uint8_t sys_id, uint8_t comp_id, uint8_t seq) +{ + int i; + uint16_t new_id = (uint16_t(sys_id) << 8) | comp_id; + + /* find source */ + for (i = 0; i < num_sources; i++) { + uint16_t old_id = (uint16_t(sources[i].sys_id) << 8) | sources[i].comp_id; + if (old_id == new_id) { + sources[i].pkt_cnt++; + sources[i].seq += 1; /* expected sequence */ + if (seq == sources[i].seq) + return 0; /* correct */ + /* sequence error */ + sources[i].err_cnt++; + sources[i].seq = seq; /* set for next time */ + return -1; + } else if (old_id > new_id) + break; /* insert new one at [i] */ + } + /* didn't find src_id, comp_id */ + /* i == num_source if we got to the end, + or is index where we want to insert */ + if (num_sources < SOURCE_MAX) { + /* add/insert new source */ + memmove(&sources[i + 1], &sources[i], (num_sources - i) * sizeof(source_info_t)); + source_set(&sources[i], sys_id, comp_id, seq); + num_sources++; + } + return 0; +} + +/* +* sources_clear - clear sources and stats +* +* This is called after each time stats are printed. There are two reasons: +* 1. So stats are deltas since the last printout, and +* 2. So if we get junk and fill up the sources[] table, it will be cleared +* the next time we print stats. +*/ +static void sources_clear(void) +{ + memset(sources, 0, sizeof(sources)); + num_sources = 0; +} + +/* +* create_and_bind - create a socket and bind it to a local UDP port +* +* Used to create the socket on the upstream side that receives from and sends +* to Solo, and the socket on the downstream side that receives from and sends +* to everything else (stm32, tlog, app, gcs). +* +* Returns fd on success, -1 on error. +*/ +int create_and_bind(short port_hbo, int tos) +{ + int fd; + struct sockaddr_in sa; + + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + perror("socket"); + return -1; + } + + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_addr.s_addr = htonl(INADDR_ANY); + sa.sin_port = htons(port_hbo); + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + perror("bind"); + close(fd); + return -1; + } + + if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) != 0) { + perror("setsockopt"); + close(fd); + return -1; + } + + return fd; + +} /* create_and_bind */ + +/* +* lock_mem - force allocation of stack and lock the pages +*/ +static void lock_mem(uint32_t size) +{ + char stack[size]; + mlockall(MCL_CURRENT | MCL_FUTURE); + memset(stack, 0, sizeof(stack)); +} + +#define TELEM_PKT_MAX 512 + +/* +* tlm_main +*/ +static void *tlm_main(void *) +{ + int fd_solo; + fd_set fds; + int nfds = 0; + int res; + uint64_t now_us; + uint8_t pkt[TELEM_PKT_MAX]; + LinkPacket packet; + uint32_t link_next_seq = 0; + struct sockaddr_in sa; + socklen_t sa_len; + int i; + uint64_t log_time_us; + uint32_t pkts_down_total = 0; + uint32_t pkts_up_total = 0; + uint32_t select_timeout = 0; + struct timeval timeout; + const uint64_t dropped_log_interval_us = + 1000000; // (1 sec) minimum time between logging drop events + uint64_t dropped_log_us = 0; // last time we logged a drop event + unsigned dropped_total = 0; // total dropped since last time we logged + + /* Solo socket. This is where downlink packets arrive. */ + fd_solo = create_and_bind(telem_dest_port_hbo, uplink_tos); + if (fd_solo < 0) { + syslog(LOG_CRIT, "can't create solo socket and bind to port %d", telem_dest_port_hbo); + return NULL; + } + + /* Clear mavlink sources and stats. */ + sources_clear(); + + /* Memory lock size should be tuned, but the value passed here is + in addition to main's stack, and it is the only thing that uses + significant stack. */ + lock_mem(32768); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + log_time_us = now_us + log_interval_us; + + /* Wait for a packet to arrive in either the upstream or downstream + direction, and forward it as needed. Also periodically check to see if + there are any new destinations on the network where the downlink should + go. Downlink goes to everyone on the network, port telem_dest_port_hbo. */ + while (1) { + uint64_t t1_us; + uint64_t t2_us; + + /* Wait for a packet, or time out if no packets arrive so we always + periodically log status and check for new destinations. Downlink + packets are on the order of 100/sec, so the timeout is such that + we don't expect timeouts unless solo stops sending packets. We + almost always get a packet with a 200 msec timeout, but not with + a 100 msec timeout. (Timeouts don't really matter though.) */ + + FD_ZERO(&fds); + nfds = 0; + FD_SET(fd_solo, &fds); + if (fd_solo >= nfds) + nfds = fd_solo + 1; + + mutex_lock(&dest_table_mutex, NULL); + + for (i = 0; i < dest_table.num_dest; i++) { + int fd = dest_table.dest[i].fd; + FD_SET(fd, &fds); + if (fd >= nfds) + nfds = fd + 1; + } + + mutex_unlock(&dest_table_mutex); + + timeout.tv_sec = 0; + timeout.tv_usec = 200000; + + t1_us = clock_gettime_us(CLOCK_MONOTONIC); + + /* The time from returning from select in loop N to getting to here in + loop N+1 should be very short. 100 msec should be far far longer + than we need; we are looking for unexpected blocks or stalls here. */ + if ((t1_us - now_us) > 100000) { + syslog(LOG_ERR, "loop: took %u usec", (unsigned)(t1_us - now_us)); + } + + /* This select is intended to be the only place this thread blocks, so + is the only place where the telemetry destinations table should get + updated. + Don't count on that (blocking has showed up in unexpected places), + but do this select while not holding the telemetry dests table mutex + in case it is int fact true. + If a telemetry dest goes away, its fd is closed, which should have + no effect on the select for Linux, but is unportable behavior. */ + res = select(nfds, &fds, NULL, NULL, &timeout); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + /* select should time out in 200 msec at the most; warn if it blocks + longer than that */ + if ((now_us - t1_us) > 250000) + syslog(LOG_ERR, "select: blocked %u usec", (unsigned)(now_us - t1_us)); + + if (res < 0) { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] select: %s", skipped, strerror(errno)); + /* this sleep is to avoid soaking the CPU if select starts + returning immediately for some reason */ + usleep(10000); + continue; + } + + if (res == 0) { + /* timeout */ + select_timeout++; + /* Could skip checking the two fds, but we need to continue on + below that, possibly checking for new telemetry destinations + or logging stats. */ + } + + /* Either have a packet ready in one of the sockets, or timed out. + Processing one packet per loop makes it so neither uplink nor + downlink can starve the other, e.g. if we get a huge burst of + downlink packets, uplink still gets to send one packet per downlink + packet if it wants to (vs no uplink at all until we work through + the downlink burst). */ + + /* check for downstream packets */ + + if (FD_ISSET(fd_solo, &fds)) { + /* packet from solo */ + sa_len = sizeof(sa); + t1_us = clock_gettime_us(CLOCK_MONOTONIC); + res = recvfrom(fd_solo, &packet, sizeof(packet), 0, (struct sockaddr *)&sa, &sa_len); + t2_us = clock_gettime_us(CLOCK_MONOTONIC); + /* recvfrom should not block; warn if it does */ + if ((t2_us - t1_us) > 10000) + syslog(LOG_ERR, "recvfrom(solo): blocked %u usec", (unsigned)(t2_us - t1_us)); + + packet.tc_recv_us = now_us; // timestamp when it was received in controller + + // initialize sequence check if this is the first downstream packet + if (link_next_seq == 0) + link_next_seq = packet.seq; + + // check sequence + // downlink packets dropped between this packet and the previous one (normally zero) + unsigned dropped = packet.seq - link_next_seq; + // total dropped since last time we logged a message + dropped_total += dropped; + + // Should we log? Yes, if we have seen drops and have not logged too recently. + if ((dropped_total > 0) && ((now_us - dropped_log_us) >= dropped_log_interval_us)) { + syslog(LOG_ERR, "dropped %u downlink packets", dropped_total); + dropped_total = 0; + dropped_log_us = now_us; + } + + link_next_seq = packet.seq + 1; + + pkts_down_total++; + + /* We get one mavlink packet per udp datagram. Sanity checks here + are: must be from solo's IP and have a valid mavlink header. */ + + if (sa.sin_addr.s_addr != sa_solo.sin_addr.s_addr) { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] received packet not from solo (0x%08x)", skipped, + sa.sin_addr.s_addr); + } else if (res < (LinkPacket::HDR_LEN + 8)) { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] received runt packet (%d bytes)", skipped, res); + } else if (packet.payload[0] != 254) { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] received bad magic (0x%02x)", skipped, packet.payload[0]); + } + /* + else if (packet.payload[1] != (res - LinkPacket::HDR_LEN - 8)) + { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] inconsistent length (%u, %u)", + skipped, packet.payload[1], res); + } + */ + else { + /* packet is from solo and passes sanity checks */ + + /* Run through the entire datagram and check each sequence and sys/comp. + * Also check for a GPS time that we can set the clock to */ + uint8_t *p_payload = packet.payload; + while (p_payload < ((uint8_t *)&packet + res)) { + uint8_t seq = p_payload[2]; + uint8_t sys_id = p_payload[3]; + uint8_t comp_id = p_payload[4]; + + source_check(sys_id, comp_id, seq); + + if (!got_gps_time && p_payload[5] == MAVLINK_MSG_ID_SYSTEM_TIME) { + // XXX hefty magic here + // usec since unix epoch is bytes 6..13, little-endian + int b; + uint64_t time_us = 0; + for (b = 13; b >= 6; b--) + time_us = time_us * 256 + p_payload[b]; + if (time_us != 0) { + char buf[32]; + syslog(LOG_INFO, "gps time %s", clock_tostr_r(time_us, buf)); + got_gps_time = 1; + if (use_gps_time) { + clock_settime_us(CLOCK_REALTIME, time_us); + syslog(LOG_INFO, "system time set from gps time"); + } + } + } + + p_payload += (8 + p_payload[1]); // Increase by the length of the payload+header + } + + /* check source port */ + if (sa_solo.sin_port == 0) { + sa_solo.sin_port = sa.sin_port; + syslog(LOG_INFO, "solo @ %s:%d", inet_ntoa(sa_solo.sin_addr), + ntohs(sa_solo.sin_port)); + } else if (sa_solo.sin_port != sa.sin_port) { + sa_solo.sin_port = sa.sin_port; + syslog(LOG_INFO, "solo is now at %s:%d", inet_ntoa(sa_solo.sin_addr), + ntohs(sa_solo.sin_port)); + } + + /* send to all destinations */ + t1_us = clock_gettime_us(CLOCK_MONOTONIC); + + mutex_lock(&dest_table_mutex, NULL); + + t2_us = clock_gettime_us(CLOCK_MONOTONIC); + /* mutex_lock should not take longer than the memcpy in + update_telem_dest; warn if it takes too long */ + if ((t2_us - t1_us) > 10000) + syslog(LOG_ERR, "mutex_lock: blocked %u usec", (unsigned)(t2_us - t1_us)); + packet.tc_send_us = clock_gettime_us(CLOCK_MONOTONIC); + for (i = 0; i < dest_table.num_dest; i++) { + // send the whole LinkPacket or just the mavlink payload + const void *buf = NULL; + ssize_t len = 0; + if (dest_table.dest[i].format == TF_MAVLINK) { + buf = (const void *)(packet.payload); + len = res - LinkPacket::HDR_LEN; + } else if (dest_table.dest[i].format == TF_LINK_PACKET) { + buf = (const void *)(&packet); + len = res; + } + /* sendto blocks if the network has gone away */ + errno = 0; + ssize_t r = sendto(dest_table.dest[i].fd, buf, len, MSG_DONTWAIT, + (struct sockaddr *)&(dest_table.dest[i].sa), + sizeof(dest_table.dest[i].sa)); + if (r != len) { + dest_table.dest[i].err_cnt++; + if ((now_us - dest_table.dest[i].err_us) >= 1000000) { + syslog(LOG_ERR, "[%u] sendto %s:%d returned %d, expected %d: %s", + dest_table.dest[i].err_cnt, + inet_ntoa(dest_table.dest[i].sa.sin_addr), + ntohs(dest_table.dest[i].sa.sin_port), r, len, strerror(errno)); + dest_table.dest[i].err_cnt = 0; + dest_table.dest[i].err_us = now_us; + } + } + } + + mutex_unlock(&dest_table_mutex); + } + } + + /* check for upstream packets */ + + mutex_lock(&dest_table_mutex, NULL); + + for (i = 0; i < dest_table.num_dest; i++) { + + int fd = dest_table.dest[i].fd; + + if (FD_ISSET(fd, &fds)) { + /* packet from gcs */ + sa_len = sizeof(sa); + t1_us = clock_gettime_us(CLOCK_MONOTONIC); + res = recvfrom(fd, pkt, sizeof(pkt), 0, (struct sockaddr *)&sa, &sa_len); + t2_us = clock_gettime_us(CLOCK_MONOTONIC); + /* recvfrom should not block; warn if it does */ + if ((t2_us - t1_us) > 10000) + syslog(LOG_ERR, "recvfrom(gcs): blocked %u usec", (unsigned)(t2_us - t1_us)); + + pkts_up_total++; + + /* send to solo */ + if (sa_solo.sin_addr.s_addr != 0 && sa_solo.sin_port != 0) { + /* sendto blocks if the network has gone away */ + if (sendto(fd_solo, pkt, res, MSG_DONTWAIT, (struct sockaddr *)&sa_solo, + sizeof(sa_solo)) != res) { + int skipped; + if ((skipped = can_log_error(now_us)) >= 0) + syslog(LOG_ERR, "[%u] sendto %s:%d: %s", skipped, + inet_ntoa(sa_solo.sin_addr), ntohs(sa_solo.sin_port), + strerror(errno)); + } + } + } + + } // for (int i...) + + mutex_unlock(&dest_table_mutex); + + /* check for time to log */ + + if (now_us >= log_time_us) { + char msg[200]; /* usually use about 40 characters */ + char *m = msg; + int n, r, s; + + memset(msg, 0, sizeof(msg)); + r = sizeof(msg) - 1; /* never overwrite that last '\0' */ + + /* In the following, snprintf will never write more than 'r' + * characters, so if we maintain that correctly and just let it + * go non-positive if we overflow, then this won't overrun. The + * return from snprint when it runs out of room is >= 'r', so 'r' + * goes zero or negative if we run out of room. */ + + n = snprintf(m, r, "dn:%d", pkts_down_total); + m += n; + r -= n; + + for (s = 0; s < num_sources; s++) { + if (r > 0) { + n = snprintf(m, r, " (%d,%d)%d/%d", sources[s].sys_id, sources[s].comp_id, + sources[s].pkt_cnt, sources[s].err_cnt); + m += n; + r -= n; + } + } + + if (r > 0) { + n = snprintf(m, r, " up:%d", pkts_up_total); + m += n; + r -= n; + } + +#if 0 + /* timeout count is only useful for tuning the timeout value */ + if (r > 0) + { + n = snprintf(m, r, " to:%d", select_timeout); + m += n; + r -= n; + } +#endif + + syslog(LOG_INFO, msg); + + sources_clear(); + pkts_down_total = 0; + pkts_up_total = 0; + select_timeout = 0; + + log_time_us += log_interval_us; + } + + } /* while (1) */ + + return NULL; + +} /* tlm_main */ + +static int start_tlm_main(void) +{ + int r; + + if ((r = pthread_create(&tlm_ctx, NULL, tlm_main, NULL)) != 0) { + syslog(LOG_ERR, "start_tlm_main: pthread_create returned %d", r); + return 1; + } + + if ((r = pthread_setname_np(tlm_ctx, "tlm_main")) != 0) { + syslog(LOG_ERR, "start_tlm_main: pthread_setname_np returned %d", r); + return 1; + } + + struct sched_param sp; + memset(&sp, 0, sizeof(sp)); + sp.sched_priority = TC_MAIN_PRIORITY; + if ((r = pthread_setschedparam(tlm_ctx, SCHED_FIFO, &sp)) != 0) { + syslog(LOG_ERR, "start_tlm_main: pthread_setschedparam returned %d", r); + return 1; + } + + return 0; + +} /* start_tlm_main */ + +int main(int argc, char *argv[]) +{ + + // + // initialize logging + // + + openlog("tlm", LOG_NDELAY, LOG_LOCAL1); + + setlogmask(LOG_UPTO(LOG_INFO)); + + syslog(LOG_INFO, "telem_ctrl starting: built " __DATE__ " " __TIME__); + + // + // read configuration + // + + INIReader reader("/etc/sololink.conf"); + if (reader.ParseError() < 0) { + syslog(LOG_CRIT, "can't parse /etc/sololink.conf"); + exit(1); + } + + std::string s1 = reader.Get("solo", "mavDestIp", "127.0.0.1"); + const char *stm32_ip = s1.c_str(); + short stm32_port_hbo = reader.GetInteger("solo", "mavDestPort", 5015); + syslog(LOG_INFO, "stm32 @ %s:%d", stm32_ip, stm32_port_hbo); + + std::string s2 = reader.Get("solo", "tlogDestIp", "127.0.0.1"); + const char *tlog_ip = s2.c_str(); + short tlog_port_hbo = reader.GetInteger("solo", "tlogDestPort", 14583); + syslog(LOG_INFO, "tlog @ %s:%d", tlog_ip, tlog_port_hbo); + + telem_dest_port_hbo = reader.GetInteger("solo", "telemDestPort", 14550); + syslog(LOG_INFO, "telem_dest_port = %d", telem_dest_port_hbo); + + use_gps_time = reader.GetBoolean("solo", "useGpsTime", true) ? 1 : 0; + syslog(LOG_INFO, "use_gps_time = %d", use_gps_time); + + uplink_tos = reader.GetInteger("solo", "telemUpTos", IP_TOS_DEFAULT); + syslog(LOG_INFO, "uplink_tos = 0x%02x", uplink_tos); + + gcs_tos = reader.GetInteger("solo", "telemGcsTos", IP_TOS_DEFAULT); + syslog(LOG_INFO, "gcs_tos = 0x%02x", gcs_tos); + + // Stations where downlink packets will be forwarded. Solo sends all + // mavlink to this process, and this process figures out who needs them + // (stm32, tlog process, app, gcs, etc.). + dest_table.init(); + mutex_init(&dest_table_mutex); + + // add internal destinations (stm32, tlog) + mutex_lock(&dest_table_mutex, NULL); + dest_table.add(NULL, inet_addr(stm32_ip), stm32_port_hbo, 0, TF_LINK_PACKET); + dest_table.add(NULL, inet_addr(tlog_ip), tlog_port_hbo); + mutex_unlock(&dest_table_mutex); + + // Solo destination socket address - the address in Solo where uplink + // packets will be sent. Fill in the family now, fill in the ip address + // from the solo ip address file (below), then finally fill in the port + // when we get any downlink (since port number on solo is dynamic). + memset(&sa_solo, 0, sizeof(sa_solo)); + sa_solo.sin_family = AF_INET; + + // This does not return until Solo is there. + wait_solo(&sa_solo.sin_addr.s_addr); + + // + // start telemetry thread + // + + if (start_tlm_main() != 0) { + syslog(LOG_ERR, "telem_ctrl: error starting tlm_main"); + exit(1); + } + + // + // periodically scan for telemetry consumers + // + + // hostapd connection. This is used to query hostapd for a list of all + // associated telemetry destinations. (Underneath, it is a unix-domain + // socket that talks to the hostapd process.) + void *hostapd_handle = hostapd_ctrl_new("wlan0-ap"); + if (hostapd_handle == NULL) { + syslog(LOG_CRIT, "can't create connection to hostapd"); + exit(1); + } + + while (true) { + + update_telem_dest(hostapd_handle); + + sleep(telem_dest_update_interval_s); + + } // while (true) + +} // main diff --git a/flightcode/telem_ctrl/telem_dest.cpp b/flightcode/telem_ctrl/telem_dest.cpp new file mode 100644 index 0000000..7529af4 --- /dev/null +++ b/flightcode/telem_ctrl/telem_dest.cpp @@ -0,0 +1,152 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "telem_dest.h" + +// +// telem_dest_table::init - initialize a telem_dest_table +// +void telem_dest_table::init(void) +{ + + memset(this, 0, sizeof(telem_dest_table)); + + for (int i = 0; i < DEST_MAX; i++) + dest[i].fd = -1; + +} // telem_dest_table::init + +// +// telem_dest_table::dump - dump a telem_dest_table to log (debug) +// +void telem_dest_table::dump(int priority) const +{ + int i; + + syslog(priority, "telem_dest_table::dump: num_dest=%d", num_dest); + for (i = 0; i < DEST_MAX; i++) { + const uint8_t *mac = dest[i].mac; + syslog(priority, "%s:%d %d %u %02x:%02x:%02x:%02x:%02x:%02x %d", + inet_ntoa(dest[i].sa.sin_addr), ntohs(dest[i].sa.sin_port), dest[i].fd, + dest[i].err_cnt, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], dest[i].format); + } + +} // telem_dest_table::dump + +// +// telem_dest_table::check - check that a telem_dest_table is not corrupt (debug) +// +// num_dest must be in range. The first num_dest entries should be nonzero in +// the ip and port fields, then entries after that should be all zeros. format +// must be valid. +// +void telem_dest_table::check(void) const +{ + int corrupt = 0; + + if (num_dest < 0 || num_dest >= DEST_MAX) { + corrupt = 1; + } else { + int i; + // entries in use + for (i = 0; i < num_dest; i++) { + if (dest[i].sa.sin_addr.s_addr == 0 || dest[i].sa.sin_port == 0 || dest[i].fd < 0) + corrupt = 1; + } + // unused entries + for (i = num_dest; i < DEST_MAX; i++) { + if (dest[i].sa.sin_addr.s_addr != 0 || dest[i].sa.sin_port != 0 || dest[i].fd != -1 || + dest[i].err_cnt != 0 || dest[i].err_us != 0 || dest[i].mac[0] != 0 || + dest[i].mac[1] != 0 || dest[i].mac[2] != 0 || dest[i].mac[3] != 0 || + dest[i].mac[4] != 0 || dest[i].mac[5] != 0) + corrupt = 1; + } + // format field + for (i = num_dest; i < DEST_MAX; i++) { + if (dest[i].format < 0 || dest[i].format >= TF_MAX) + corrupt = 1; + } + } + + // dump if corrupt + if (corrupt) { + syslog(LOG_CRIT, "telem dest table is corrupt"); + dump(LOG_CRIT); + } + +} // telem_dest_table::check + +extern int create_and_bind(short src_port_hbo, int tos); + +// +// telem_dest_table::add - add a destination to the list of destinations receiving mavlink +// +// ip and port parameters are in network byte order. +// +// Returns 0 on success, or -1 on error (no room in table). +// +int telem_dest_table::add(const uint8_t *mac, const in_addr_t dest_ip, const short dest_port_hbo, + int tos, telem_format_t format) +{ + + if (num_dest >= DEST_MAX) + return -1; // table full + + int fd = create_and_bind(0, tos); // any local port + if (fd < 0) + return -1; + + dest[num_dest].fd = fd; + + dest[num_dest].format = format; + + // mac will be NULL for local destinations (stm32, tlog) + if (mac != NULL) + memcpy(dest[num_dest].mac, mac, MAC_LEN); + + dest[num_dest].sa.sin_addr.s_addr = dest_ip; + dest[num_dest].sa.sin_port = htons(dest_port_hbo); + + num_dest++; + + return 0; + +} // telem_dest_table::add + +// +// telem_dest_table::delete_by_index - delete a destination at a specified index +// +void telem_dest_table::delete_by_index(int index) +{ + + (void)close(dest[index].fd); + + for (int i = index; i < (DEST_MAX - 1); i++) + memcpy(&dest[i], &dest[i + 1], sizeof(telem_dest)); + + memset(&dest[DEST_MAX - 1], 0, sizeof(telem_dest)); + dest[DEST_MAX - 1].fd = -1; + + num_dest--; + +} // telem_dest_table::delete_by_index + +// +// telem_dest_table::find_by_mac_ip - find a destination with a specified mac and IP +// +int telem_dest_table::find_by_mac_ip(const uint8_t *mac, const in_addr_t ip) const +{ + int i; + + for (i = 0; i < num_dest; i++) + if (memcmp(mac, dest[i].mac, MAC_LEN) == 0 && ip == dest[i].sa.sin_addr.s_addr) + return i; + + return -1; + +} // telem_dest_table::find_by_mac_ip diff --git a/flightcode/telem_ctrl/telem_dest.h b/flightcode/telem_ctrl/telem_dest.h new file mode 100644 index 0000000..a338bd8 --- /dev/null +++ b/flightcode/telem_ctrl/telem_dest.h @@ -0,0 +1,55 @@ +#ifndef TELEM_DEST_H +#define TELEM_DEST_H + +// +// telemetry destinations table +// +// This is the destinations that receive mavlink. It is updated periodically +// based on who is associated with the AP and who is in the ARP table. The +// table's first 'num_dest' entries are in use, and all entries after that are +// free. An entry is added by putting it at index 'num_dest'. An entry 'k' is +// deleted by copying all entries from 'k' + 1 to the end down one slot +// (overwriting the entry to be deleted) and clearing the one now at the end. +// + +#ifndef MAC_LEN +#define MAC_LEN 6 +#endif + +// Each entry either wants raw mavlink, or a link_packet (mavlink with +// metadata wrapper). +typedef enum { + TF_MAVLINK, // raw mavlink + TF_LINK_PACKET, // link_packet.h + TF_MAX // (last) +} telem_format_t; + +// one entry in table +struct telem_dest { + telem_format_t format; + int fd; + sockaddr_in sa; + unsigned err_cnt; // send errors for this dest since last message + uint64_t err_us; // time of last send error message + uint8_t mac[MAC_LEN]; +}; + +// the table (entries plus count) +struct telem_dest_table { + // Maximum number of destinations. Normally we have up to four: + // two internal (stm32 and tlog), maybe an app, and maybe a gcs. + static const int DEST_MAX = 32; + + telem_dest dest[DEST_MAX]; + int num_dest; + + void init(void); + void dump(int priority) const; + void check(void) const; + int add(const uint8_t *mac, const in_addr_t ip, const short port_hbo, int tos = 0, + telem_format_t format = TF_MAVLINK); + void delete_by_index(int index); + int find_by_mac_ip(const uint8_t *mac, const in_addr_t ip) const; +}; + +#endif // TELEM_DEST_H diff --git a/flightcode/telem_test/Makefile b/flightcode/telem_test/Makefile new file mode 100644 index 0000000..09538c6 --- /dev/null +++ b/flightcode/telem_test/Makefile @@ -0,0 +1,36 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util ../mavlink/c_library/ardupilotmega + +INCS = -I../util -I../mavlink/c_library/ardupilotmega + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS = -lpthread + +SRCS_CPP = telem_test.cpp +SRCS_C = util.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = telem_test + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +.PHONY: clean diff --git a/flightcode/telem_test/telem_test.cpp b/flightcode/telem_test/telem_test.cpp new file mode 100644 index 0000000..70b7203 --- /dev/null +++ b/flightcode/telem_test/telem_test.cpp @@ -0,0 +1,351 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink.h" + + +#define SERIAL_DEV_NAME "/dev/ttymxc1" + + +// Needs to match pixhawk's setting +#define SERIAL_DEV_BAUD 921600 +#define SERIAL_DEV_BAUD_CODE B921600 +//#define SERIAL_DEV_BAUD 460800 +//#define SERIAL_DEV_BAUD_CODE B460800 +//#define SERIAL_DEV_BAUD 230400 +//#define SERIAL_DEV_BAUD_CODE B230400 +//#define SERIAL_DEV_BAUD 115200 +//#define SERIAL_DEV_BAUD_CODE B115200 + +#define SERIAL_TX_BUF_SIZE 4096 + +// MAVLINK_MESSAGE_INFO has 256 elements; sizeof(mavlink_info)=528384 +//static mavlink_message_info_t mavlink_info[] = MAVLINK_MESSAGE_INFO; + +// MAVLINK_MESSAGE_CRCS is an array of 256 bytes +static uint8_t mavlink_crc[] = MAVLINK_MESSAGE_CRCS; + +// A mavlink packet is limited to 6+255+2 = 263 bytes +// 6 byte header, 255 max bytes in payload, 2 byte crc + +static volatile bool restart = false; + +static pthread_t downstream_id; + + +// mode = O_RDONLY or O_WRONLY +static int serial_setup(int mode) +{ + int fd; + struct termios options; + + fd = open(SERIAL_DEV_NAME, mode | O_NOCTTY | O_NONBLOCK); + if (fd < 0) + { + perror("open"); + return -1; + } + + tcgetattr(fd, &options); + + cfsetspeed(&options, SERIAL_DEV_BAUD_CODE); + + options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + options.c_cflag &= ~(CSIZE | PARENB); + options.c_cflag |= CS8; +#if 1 + options.c_cflag |= CLOCAL; + options.c_cflag &= ~(CRTSCTS); +#else + options.c_cflag |= CRTSCTS; +#endif + options.c_cc[VMIN] = 17; + options.c_cc[VTIME] = 0; + + tcsetattr(fd, TCSANOW, &options); + + return fd; + +} // serial_setup + + +// Downstream thread reads from the serial port, doing just enough mavlink +// synchronization to allow packet counting. The only thing packet counting +// does is verify we're at the right baud rate. +static void* downstream(void* arg) +{ + const unsigned READ_MAX = 500; + uint8_t data_in[READ_MAX]; + unsigned data_in_bytes = 0; + unsigned bytes_dropped = 0; + unsigned pkts_received = 0; + const int nfds = 1; + struct pollfd fds[nfds]; + int dn_fd = -1; + + dn_fd = serial_setup(O_RDONLY); + if (dn_fd < 0) + exit(1); + printf("dn_fd=%d\n", dn_fd); + + while (1) + { + + if (restart) + { + fprintf(stderr, "down: restarting...\n"); + close(dn_fd); + fprintf(stderr, "down: restart = false\n"); + restart = false; + dn_fd = serial_setup(O_RDONLY); + if (dn_fd < 0) + exit(1); + printf("new dn_fd=%d\n", dn_fd); + } + + fds[0].fd = dn_fd; + fds[0].events = POLLIN; + + if (poll(fds, nfds, -1) <= 0) + { + perror("poll"); + sleep(1); + continue; + } + + unsigned read_max = READ_MAX - data_in_bytes; + int num_read = read(dn_fd, data_in + data_in_bytes, read_max); + + if (num_read == 0) + { + // eof + break; + } + + if (num_read < 0) + { + // error + perror("read"); + break; + } + + data_in_bytes += num_read; + + bool emitted; + + do + { + + emitted = false; + + // Throw away data from the start until there's magic in the right + // place. This loop should almost never have to execute. + while (data_in_bytes > 0 && data_in[0] != MAVLINK_STX) + { + memmove(data_in, data_in + 1, --data_in_bytes); + bytes_dropped++; + } + + // consume packet if we have one + if (data_in_bytes > 1 && data_in_bytes >= (unsigned)(6 + data_in[1] + 2)) + { + pkts_received++; + unsigned consume_bytes = 6 + data_in[1] + 2; + data_in_bytes -= consume_bytes; + memmove(data_in, data_in + consume_bytes, data_in_bytes); + emitted = true; + + if (pkts_received >= 100) + { + printf("%d packets received, %d bytes dropped\n", pkts_received, bytes_dropped); + pkts_received -= 100; + } + } + + } + while (emitted); + + } // while (1) + + return NULL; + +} // downstream + + +// calc_crc - calculate the CRC of a mavlink message +// +// CRC is seeded, then starts with the byte after the magic, continues through +// the payload up to the CRC in the message, then add the message-id-specific +// byte. +// +// Returns the 16-bit CRC. +static uint16_t calc_crc(const uint8_t* p) +{ + uint8_t payload_len = p[1]; + uint8_t msg_id = p[5]; + unsigned crc_len = 5 + payload_len; + + uint16_t crc = crc_calculate(p + 1, crc_len); + + crc_accumulate(mavlink_crc[msg_id], &crc); + + return crc; +} + + +// set_crc - set the CRC in a mavlink message +static void set_crc(uint8_t* p, uint16_t crc) +{ + unsigned payload_len = p[1]; + p[6 + payload_len] = crc; + p[6 + payload_len + 1] = crc >> 8; +} + + +// get_crc - get (extract) the CRC of a mavlink message +// +// Returns the 16-bit CRC. +static uint16_t get_crc(const uint8_t* p) +{ + uint8_t payload_len = p[1]; + + uint16_t crc = p[6 + payload_len + 1]; // msb + crc <<= 8; + crc |= p[6 + payload_len]; // lsb + + return crc; + +} // get_crc + + +// check_crc - check the crc of a mavlink message +// +// Returns true if CRC correct, else false. +static bool check_crc(const uint8_t* p) +{ + return calc_crc(p) == get_crc(p); +} + + +// Start downstream thread, then do upstream loop here. +// The upstream loop just sends the same mavlink packet out as fast as it can. +int main(int argc, char* argv[]) +{ + int num_bytes; + int tot_bytes = 0; + uint8_t msg[300]; + int msg_bytes; + uint16_t crc; + const int nfds = 1; + struct pollfd fds[nfds]; + int poll_rc; + int up_fd = -1; + + pthread_create(&downstream_id, NULL, downstream, NULL); + + memset(&msg, 0, sizeof(msg)); +#if 0 + // version request message + msg[0] = MAVLINK_STX; // sync + msg[1] = MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN; // msg length + msg[2] = 0; // sequence + msg[3] = 10; // src id + msg[4] = 10; // src component + msg[5] = MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST; // msg id + msg[6] = 1; // target system + msg[7] = 1; // target component + msg_bytes = 6 + MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST_LEN + 2; +#elif 1 + // encapsulated data message + msg[0] = MAVLINK_STX; // sync + msg[1] = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; // msg length + msg[2] = 0; // sequence + msg[3] = 10; // src id + msg[4] = 10; // src component + msg[5] = MAVLINK_MSG_ID_ENCAPSULATED_DATA; // msg id + msg_bytes = 6 + MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN + 2; +#endif + + crc = calc_crc(msg); + set_crc(msg, crc); + if (!check_crc(msg)) + { + fprintf(stderr, "CRC ERROR\n"); + exit(1); + } + + up_fd = serial_setup(O_WRONLY); + if (up_fd < 0) + exit(1); + printf("up_fd=%d\n", up_fd); + + fds[0].fd = up_fd; + fds[0].events = POLLOUT; + + while (1) + { + + poll_rc = poll(fds, nfds, 1000); + if (poll_rc < 0) + { + perror("poll"); + sleep(1); + continue; + } + else if (poll_rc == 0) + { + // timeout + fprintf(stderr, "up: write stuck\n"); + + // super hacky restart + close(up_fd); + restart = true; + fprintf(stderr, "up: waiting for restart false...\n"); + // wait for downstream to see restart and close its file descriptor + while (restart) + usleep(100000); + fprintf(stderr, "up: ok\n"); + // downstream is now reopening its file descriptor + up_fd = serial_setup(O_WRONLY); + if (up_fd < 0) + exit(1); + printf("new up_fd=%d\n", up_fd); + fds[0].fd = up_fd; + continue; + } + + num_bytes = write(up_fd, &msg, msg_bytes); + + tot_bytes += num_bytes; + + if (num_bytes < msg_bytes) + { + fprintf(stderr, "write: returned %d at tot_bytes=%d\n", num_bytes, tot_bytes); + sleep(1); + continue; + } + +#define PRINT_COUNT 5000 + if (tot_bytes >= PRINT_COUNT) + { + printf("sent %d bytes\n", PRINT_COUNT); + tot_bytes -= PRINT_COUNT; + } + + } // while (1) + +} // main diff --git a/flightcode/thermal/log_temp b/flightcode/thermal/log_temp new file mode 100755 index 0000000..8ea8f3c --- /dev/null +++ b/flightcode/thermal/log_temp @@ -0,0 +1,8 @@ +#!/bin/bash + +TEMPFILE=/sys/class/thermal/thermal_zone0/temp + +while true; do + logger -t temp -p local5.info -- `cat $TEMPFILE` + sleep 60 +done diff --git a/flightcode/tlog/Makefile b/flightcode/tlog/Makefile new file mode 100644 index 0000000..ecbcead --- /dev/null +++ b/flightcode/tlog/Makefile @@ -0,0 +1,45 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../log ../util + +INCS = -I../log -I../util + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +SRCS_CPP = tlog.cpp +SRCS_CPP += Log.cpp +SRCS_C += util.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = tlog + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) + +clean: + $(RM) *.o *~ $(MAIN) + $(RM) ../log/*.o + $(RM) ../util/*.o + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/tlog/tlog.cpp b/flightcode/tlog/tlog.cpp new file mode 100644 index 0000000..c6c752b --- /dev/null +++ b/flightcode/tlog/tlog.cpp @@ -0,0 +1,312 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Log.h" +#include "../mavlink/c_library/common/mavlink.h" +#include "../mavlink/c_library/ardupilotmega/mavlink.h" +#include "util.h" + +using namespace std; + +/*********************************************************************** +UDP macros +***********************************************************************/ +#define BUFSIZE 4096 + +/*********************************************************************** +File descriptors +***********************************************************************/ +int sock_fd; + +/*********************************************************************** +TLOG source port from telem_ctrl.py +***********************************************************************/ +#define TLOG_SOURCE_PORT 14583 + +/*********************************************************************** +Log file +***********************************************************************/ +Log *logfile; + +/*********************************************************************** +Max log file size +***********************************************************************/ +#define MAX_LOGFILE_SIZE 100000000 // 100MB +#define MAX_LOG_FILES 9 + +/*********************************************************************** +Log check timeout +***********************************************************************/ +#define LOG_CHECK_US 5000000 // Check every 5s + +/*********************************************************************** + Logfile directory and name + (directory can come from environment) +***********************************************************************/ +#define LOGFILE_DIR "/log" +#define LOGFILE_NAME "solo.tlog" + +/*********************************************************************** +Function: int UDP_setup(void) + +Description: Sets up the UDP receive port. Returns 0 in the event of + an error, 1 otherwise. +***********************************************************************/ +int UDP_setup(void) +{ + struct sockaddr_in myaddr; /* our address */ + struct timeval timeout; + int sourcePort = TLOG_SOURCE_PORT; + + /* create a UDP socket */ + if ((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + cerr << "cannot create socket" << endl; + return 0; + } + + /* Socket timeout */ + timeout.tv_sec = 0; + timeout.tv_usec = 1000; // 1ms + setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout)); + + /* Bind the socket to any IP, but we'll check the source later */ + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(sourcePort); + + if (bind(sock_fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + cerr << "bind failed" << endl; + return 0; + } + + cout << "Successfully opened port " << sourcePort << endl; + return 1; +} + +/*********************************************************************** +Function: void dump_versions(void) + +Description: Reads the sololink, artoolink, artoo, and pixhawk versions + and dumps them into the tlog as statustext +***********************************************************************/ +void dump_versions(void) +{ + string s_sl_ver("UNK"); // Sololink version + string s_pix_ver("UNK"); // Pixhawk version + string s_al_ver("UNK"); // Artoolink version + string s_a_ver("UNK"); // Artoo (STM32) version + string s_all_vers; // The whole statustext string + char stringbuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; // String buffer for mav pack + ifstream infile; // File for reading + mavlink_message_t mav_msg; // Mavlink message for statustext versions + uint64_t usec; // Timestamp + char msg[BUFSIZE + 8]; // Message including timestamp to dump to file + unsigned char buf[BUFSIZE]; // mavlink message in char buffer + unsigned len; + + // Read the Pixhawk verison number + infile.open("/tmp/PEER_FW_VERSION", ifstream::in); + if (!infile) + cerr << "Unable to open pixhawk firmware version file" << endl; + else { + getline(infile, s_pix_ver); + if (!infile) + cerr << "Error reading Pixhawk firmware version" << endl; + infile.close(); + } + + // Read the SoloLink version number + infile.open("/tmp/PEER_SL_VERSION", ifstream::in); + if (!infile) + cerr << "Unable to open Sololink firmware version file" << endl; + else { + getline(infile, s_sl_ver); + if (!infile) + cerr << "Error reading Sololink firmware version" << endl; + infile.close(); + } + + // Read the Artoolink version number + infile.open("/VERSION", ifstream::in); + if (!infile) + cerr << "Unable to open Artoolink firmware version file" << endl; + else { + getline(infile, s_al_ver); + if (!infile) + cerr << "Error reading Artoolink firmware version" << endl; + infile.close(); + } + + // Read the Artoo (STM32) version number + infile.open("/STM_VERSION", ifstream::in); + if (!infile) + cerr << "Unable to open Artoo STM32 firmware version file" << endl; + else { + getline(infile, s_a_ver); + if (!infile) + cerr << "Error reading Artoo STM32 firmware version" << endl; + infile.close(); + } + + // Create a STATUSTEXT mavlink message to dump in the log. + s_all_vers = "P:" + s_pix_ver + " SL:" + s_sl_ver + " AL:" + s_al_ver + " A:" + s_a_ver; + + // Copy 49 characters (save room for a null terminator), truncating. + memset(stringbuf, 0, sizeof(stringbuf)); + len = s_all_vers.length(); + if (len >= 50) + len = 49; + memcpy(stringbuf, s_all_vers.c_str(), len); + + // Pack into a statustext message + mavlink_msg_statustext_pack(0, 0, &mav_msg, MAV_SEVERITY_INFO, stringbuf); + + // Pull the char buffer out of the mavlink message (len reused here) + len = mavlink_msg_to_send_buffer((uint8_t *)buf, &mav_msg); + + // This should be thread safe, it happens atomically in UDP_task or + // before UDP_task is started + usec = clock_gettime_us(CLOCK_REALTIME); + + for (int i = 0; i < 8; ++i) + msg[i] = usec >> (8 - (i + 1)) * 8; + + memcpy(msg + sizeof(uint64_t), buf, len); + logfile->log_fd.write((const char *)msg, len + sizeof(uint64_t)); + logfile->log_fd.flush(); +} + +/*********************************************************************** +Function: void UDP_task(void) + +Description: The main UDP task. Waits for data on the telem UDP + and just dumps it into the tlog file. +***********************************************************************/ +void UDP_task(void) +{ + int recvlen; /* # bytes received */ + unsigned char buf[BUFSIZE]; /* receive buffer */ + unsigned char *buf_ptr; + int msglen; + struct sockaddr_in srcAddr; + socklen_t slen = sizeof(srcAddr); + uint64_t usec; + char msg[BUFSIZE + 8]; + mavlink_message_t mav_msg; + mavlink_status_t mav_status; + mavlink_heartbeat_t heartbeat; + bool armedState = false; + uint64_t last, now; + + last = clock_gettime_us(CLOCK_MONOTONIC); + + while (true) { + // Attempt to receive data + recvlen = recvfrom(sock_fd, buf, BUFSIZE, 0, (sockaddr *)&srcAddr, &slen); + + // TODO:we should check that this only comes from the Artoo + if (recvlen > 0) { + /* We can get multiple messages per udp datagram, so run through them all */ + buf_ptr = buf; + while (1) { + /*Make sure this is a mavlink message. If its not, bail out of this whole thing */ + if (buf_ptr[0] != 0xFE) + break; + + msglen = buf_ptr[1] + 8; + + // If we get a "disarm" from MAVLINK, roll the log here + if (buf_ptr[5] == MAVLINK_MSG_ID_HEARTBEAT) { + for (int i = 0; i < msglen; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, buf_ptr[i], &mav_msg, &mav_status)) { + mavlink_msg_heartbeat_decode(&mav_msg, &heartbeat); + if (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (!armedState) + armedState = true; + } else { + if (armedState) { + logfile->forceRoll(); + dump_versions(); // Dump the versions into each new file + armedState = false; + } + } + } + } + } + + usec = clock_gettime_us(CLOCK_REALTIME); + + for (int i = 0; i < 8; ++i) + msg[i] = usec >> (8 - (i + 1)) * 8; + + memcpy(msg + sizeof(uint64_t), buf_ptr, msglen); + + logfile->log_fd.write((const char *)msg, msglen + sizeof(uint64_t)); + logfile->log_fd.flush(); + + buf_ptr += msglen; + if (buf_ptr >= (buf + recvlen)) + break; + } + } + + now = clock_gettime_us(CLOCK_MONOTONIC); + + // Check if we need to roll the logfile + if ((now - last) > LOG_CHECK_US) { + logfile->checkSizeAndRoll(); + last = now; + } + } +} + +/********************************************************************** +Function: int main(void) + +Description: The main function. Initializes and runs the serial and + UDP threads. +***********************************************************************/ +int main(void) +{ + logfile = new Log("/log/solo.tlog", MAX_LOGFILE_SIZE, MAX_LOG_FILES, true); + + if (!logfile->log_fd) { + cerr << "Unable to open logfile" << endl; + return -1; + } + + // Dump the versions of each system into the log file + dump_versions(); // Dump the versions into each new file + + /* Set up the UDP and serial ports */ + if (!UDP_setup()) { + cerr << "Unable to initialize the UDP receive" << endl; + return -1; + } + + // We'll enter an infinite loop here + UDP_task(); + + return -1; +} diff --git a/flightcode/unlock/Makefile b/flightcode/unlock/Makefile new file mode 100644 index 0000000..16e468b --- /dev/null +++ b/flightcode/unlock/Makefile @@ -0,0 +1,37 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../util + +INCS = -I../util + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +BIN = unlock + +all: $(BIN) + +$(BIN): unlock.o ButtonEventMessage.o RcLock.o file_util.o + $(LINK.cpp) -o $(BIN) unlock.o ButtonEventMessage.o RcLock.o file_util.o + +clean: + $(RM) *.o *~ $(BIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/unlock/unlock.cpp b/flightcode/unlock/unlock.cpp new file mode 100644 index 0000000..34ccbfb --- /dev/null +++ b/flightcode/unlock/unlock.cpp @@ -0,0 +1,104 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ButtonEventMessage.h" +#include "RcLock.h" + +// This program is expected to run once at startup. Its job is to notice RC +// is currently locked, and if so, wait for the magic button combo and unlock +// RC if it the magic combo ever seen. +// +// Magic combo is a long-press on A, B, Fly, and Home. +// +// Processing: +// 1. If RC is already unlocked, exit. +// 2. Otherwise, connect to the button event server in the controller and +// start waiting for button events. +// 3. For each message, save the most recent event for each button. +// 4. When the message says the magic combo buttons are down, and the most +// recent event for each of those buttons is "long press", unlock RC and +// exit. + +// IP:port of button event server +static const char *server_ip = "10.1.1.1"; +static const int server_port = 5016; + +// button mask for the magic combo, to be matched against 'allButtons' in +// button event message +static const uint16_t magic_buttons = + (1 << ButtonEventMessage::ButtonFly) | (1 << ButtonEventMessage::ButtonRTL) | + (1 << ButtonEventMessage::ButtonA) | (1 << ButtonEventMessage::ButtonB); + +int main(int argc, char *argv[]) +{ + int s; + struct sockaddr_in sa; + ButtonEventMessage msg; + // save most recent event for each button + int btn_event[ButtonEventMessage::ButtonMax]; + + openlog("unlock", LOG_NDELAY, LOG_LOCAL1); + + syslog(LOG_INFO, "built " __DATE__ " " __TIME__); + + if (!RcLock::locked()) { + syslog(LOG_INFO, "rc is not locked"); + return 1; + } + + s = socket(AF_INET, SOCK_STREAM, 0); + if (s < 0) { + syslog(LOG_ERR, "socket: %s", strerror(errno)); + return 1; + } + + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(server_port); + sa.sin_addr.s_addr = inet_addr(server_ip); + syslog(LOG_INFO, "connecting to %s:%d", server_ip, server_port); + while (1) { + if (connect(s, (struct sockaddr *)&sa, sizeof(sa)) == 0) + break; + sleep(1); + } + + // okay to init to 0 ("Press") since we are looking for LongHold + memset(&btn_event, 0, sizeof(btn_event)); + + // The following assumes button messages are not split across recv calls. + // They can in theory be split across recv calls, but it would probably + // have to be forced somehow. + syslog(LOG_INFO, "waiting for unlock"); + while (1) { + if (recv(s, &msg, sizeof(msg), 0) == sizeof(msg)) { + // std::cout << msg << std::endl; + + // accumulate most recent event for each button + btn_event[msg.id] = msg.event; + + // magic combo is all down, and they are all long-press + if (msg.allButtons == magic_buttons && + btn_event[ButtonEventMessage::ButtonFly] == ButtonEventMessage::LongHold && + btn_event[ButtonEventMessage::ButtonRTL] == ButtonEventMessage::LongHold && + btn_event[ButtonEventMessage::ButtonA] == ButtonEventMessage::LongHold && + btn_event[ButtonEventMessage::ButtonB] == ButtonEventMessage::LongHold) { + syslog(LOG_INFO, "unlocked"); + RcLock::unlock_override(); + shutdown(s, SHUT_RDWR); + close(s); + return 0; + } + } else { + sleep(1); + } + } + +} // main diff --git a/flightcode/util/ButtonEventMessage.cpp b/flightcode/util/ButtonEventMessage.cpp new file mode 100644 index 0000000..c7f4474 --- /dev/null +++ b/flightcode/util/ButtonEventMessage.cpp @@ -0,0 +1,15 @@ +#include +#include +#include "ButtonEventMessage.h" + +using namespace std; + +// Send a ButtonEventMessage to a stream in a friendly format. +ostream &operator<<(ostream &os, const struct ButtonEventMessage &msg) +{ + os << "timestamp=" << msg.timestamp; + os << " id=" << msg.idName(); + os << " event=" << msg.eventName(); + os << " allButtons=" << setfill('0') << setw(4) << hex << msg.allButtons; + return os; +} diff --git a/flightcode/util/ButtonEventMessage.h b/flightcode/util/ButtonEventMessage.h new file mode 100644 index 0000000..bfb28ca --- /dev/null +++ b/flightcode/util/ButtonEventMessage.h @@ -0,0 +1,75 @@ +#ifndef BUTTON_EVENT_MESSAGE_H +#define BUTTON_EVENT_MESSAGE_H + +#include +#include + +struct __attribute((__packed__)) ButtonEventMessage { + // These four fields are expected to be the only data and in this order, + // so the address of the structure can be interpreted as a 12-byte char + // array with the fields in native byte order (little-endian on ARM). + uint64_t timestamp; + uint8_t id; + uint8_t event; + uint16_t allButtons; + + // match artoo/src/io.h + enum ButtonId { + ButtonPower, + ButtonFly, + ButtonRTL, + ButtonLoiter, + ButtonA, + ButtonB, + ButtonPreset1, + ButtonPreset2, + ButtonCameraClick, + ButtonMax + }; + + inline const char *idName(void) const + { + static const char *names[] = { + "Power", "Fly", "RTL", "Loiter", "A", "B", "Preset1", "Preset2", "CameraClick", + }; + if (id >= 0 && id < ButtonMax) + return names[id]; + else + return "INVALID"; + } + + // match artoo/src/button.h + enum ButtonEvent { Press, Release, ClickRelease, Hold, LongHold, DoubleClick, EventMax }; + + inline const char *eventName(void) const + { + static const char *names[] = { + "Press", "Release", "ClickRelease", "Hold", "LongHold", "DoubleClick", + }; + if (event >= 0 && event < EventMax) + return names[event]; + else + return "INVALID"; + } + + ButtonEventMessage(void) + { + timestamp = 0; + id = 0; + event = 0; + allButtons = 0; + } + + ButtonEventMessage(uint64_t ts, const char *rawMsg) + { + timestamp = ts; + // rawMsg is ButtonEvent in artoo/src/buttonmanager.h + id = rawMsg[0]; + event = rawMsg[1]; + allButtons = rawMsg[2] | (rawMsg[3] << 8); // little endian + } +}; + +std::ostream &operator<<(std::ostream &os, const struct ButtonEventMessage &msg); + +#endif // BUTTON_EVENT_MESSAGE_H diff --git a/flightcode/util/Makefile b/flightcode/util/Makefile new file mode 100644 index 0000000..a957488 --- /dev/null +++ b/flightcode/util/Makefile @@ -0,0 +1,35 @@ + +CFLAGS += -Wall + +syslog_test.o : CFLAGS += -Wno-format-security + +syslog_test.o syslog.o : CFLAGS += -D SYSLOG_SOCK_NAME=\"/tmp/syslog_test\" +syslog_test.o syslog.o : CFLAGS += -D SYSLOG_MSG_MAX=64 + +CFLAGS += $(INCS) + +SRCS_C = main.c \ +util.c util_test.c \ +syslog.c syslog_test.c + +OBJS = $(SRCS_C:.c=.o) + +MAIN = util_test + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/util/RcLock.cpp b/flightcode/util/RcLock.cpp new file mode 100644 index 0000000..d76ab57 --- /dev/null +++ b/flightcode/util/RcLock.cpp @@ -0,0 +1,48 @@ + +#include +#include +#include +#include "file_util.h" +#include "RcLock.h" + +namespace RcLock +{ + +// must match rc_lock.py +static const char *ro_lockfile = "/mnt/rootfs.ro/etc/.rc_lock"; +static const char *tmp_lockfile = "/tmp/.rc_lock"; +static const char *tmp_unlockfile = "/tmp/.rc_unlock"; + +// locked - see if RC is locked +// +// Returns true if RC is locked, false if not locked. +bool locked(void) +{ + + if (file_exists(tmp_unlockfile)) + return false; + else if (file_exists(tmp_lockfile) || file_exists(ro_lockfile)) + return true; + else + return false; +} + +// lock_version - lock RC due to version mismatch +void lock_version(void) +{ + file_touch(tmp_lockfile); +} + +// unlock_version - unlock RC (versions match) +void unlock_version(void) +{ + unlink(tmp_lockfile); +} + +// unlock_override - unlock RC +void unlock_override(void) +{ + file_touch(tmp_unlockfile); +} + +} // namespace RcLock diff --git a/flightcode/util/RcLock.h b/flightcode/util/RcLock.h new file mode 100644 index 0000000..2fc0a1b --- /dev/null +++ b/flightcode/util/RcLock.h @@ -0,0 +1,14 @@ +#ifndef RC_LOCK_H +#define RC_LOCK_H + +namespace RcLock +{ + +extern bool locked(void); +extern void lock_version(void); +extern void unlock_version(void); +extern void unlock_override(void); + +} // namespace RcLock + +#endif // RC_LOCK_H diff --git a/flightcode/util/arp_table.c b/flightcode/util/arp_table.c new file mode 100644 index 0000000..adcde07 --- /dev/null +++ b/flightcode/util/arp_table.c @@ -0,0 +1,171 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "arp_table.h" + +/* +* arp_table_get - get arp table +* +* Read /proc/net/arp and fill in arp_table. +* +* /proc/net/arp: +* IP address HW type Flags HW address Mask Device +* 10.1.1.112 0x1 0x2 04:8d:38:7f:1e:20 * wlan0-ap +* 10.1.1.10 0x1 0x2 00:1f:09:04:00:22 * wlan0-ap +* +* On success, arp_table is filled in with arp entries, arp_entries is set to +* the number of entries, and zero is returned. On error, nonzero is returned. +*/ +int arp_table_get(arp_entry_t *arp_table, int *arp_entries) +{ + int status = 0; + FILE *fp; + char linebuf[200]; + int entries; + + if (arp_table == NULL || arp_entries == NULL) + return -1; + + fp = fopen("/proc/net/arp", "r"); + if (fp == NULL) + return -1; + + memset(arp_table, 0, *arp_entries * sizeof(arp_entry_t)); + entries = 0; + + /* skip first line */ + fgets(linebuf, sizeof(linebuf), fp); + + /* read entries, one per line */ + while (fgets(linebuf, sizeof(linebuf), fp) != NULL) { + char *saveptr; + char *token; + unsigned token_len; + char *endptr; + unsigned token_val; + struct in_addr ip; + + /* get first token (IP address, "10.1.1.112") */ + token = strtok_r(linebuf, " ", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting first token in line */ + } + token_len = strlen(token); + if (token_len == 0 || !inet_aton(token, &ip)) { + status = -1; + break; /* error parsing token */ + } + arp_table[entries].ip = ip.s_addr; + + /* get next token (HW type, "0x1") */ + token = strtok_r(NULL, " ", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting next token in line */ + } + endptr = NULL; + token_val = strtol(token, &endptr, 0); + if (*endptr != '\0') { + status = -1; + break; /* error converting token */ + } + arp_table[entries].hw_type = token_val; + + /* get next token (Flags, "0x2") */ + token = strtok_r(NULL, " ", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting next token in line */ + } + endptr = NULL; + token_val = strtol(token, &endptr, 0); + if (*endptr != '\0') { + status = -1; + break; /* error converting token */ + } + arp_table[entries].flags = token_val; + + /* get HW address ("04:8d:38:7f:1e:20") */ + token = strtok_r(NULL, " ", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting next token in line */ + } + if (mac_aton(token, arp_table[entries].mac) == NULL) { + status = -1; + break; /* error converting token to mac address */ + } + + /* get next token (Mask, "*") */ + token = strtok_r(NULL, " ", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting next token in line */ + } + /* token not used */ + + /* get next token (Device, "wlan0-ap") */ + token = strtok_r(NULL, " \n", &saveptr); + if (token == NULL) { + status = -1; + break; /* error getting next token in line */ + } + strncpy(arp_table[entries].dev, token, DEV_NAME_MAX - 1); + + entries++; + if (entries >= *arp_entries) { + /* return success (leave status = 0) */ + break; /* filled up user-supplied table */ + } + + } /* while (fgets...) */ + + fclose(fp); + + *arp_entries = entries; + + return status; + +} /* arp_table_get */ + +/* +* arp_table_find_ip - find entry in arp table using ip as key +* +* Returns index of entry, or -1 if not found. +*/ +int arp_table_find_by_ip(arp_entry_t *arp_table, int arp_entries, in_addr_t ip) +{ + int i; + + for (i = 0; i < arp_entries; i++) + if (ip == arp_table[i].ip) + return i; + + return -1; + +} /* arp_table_find_ip */ + +void arp_table_dump(int priority, const arp_entry_t *arp_table, int arp_entries) +{ + int i; + + syslog(priority, "arp_table:"); + + for (i = 0; i < arp_entries; i++) { + struct in_addr in; + in.s_addr = arp_table[i].ip; + const uint8_t *mac = arp_table[i].mac; + syslog(priority, "%-15s 0x%x 0x%x %02x:%02x:%02x:%02x:%02x:%02x %s\n", inet_ntoa(in), + arp_table[i].hw_type, arp_table[i].flags, mac[0], mac[1], mac[2], mac[3], mac[4], + mac[5], arp_table[i].dev); + } + +} /* arp_table_dump */ diff --git a/flightcode/util/arp_table.h b/flightcode/util/arp_table.h new file mode 100644 index 0000000..2c6ef82 --- /dev/null +++ b/flightcode/util/arp_table.h @@ -0,0 +1,32 @@ +#ifndef ARP_TABLE_H +#define ARP_TABLE_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAC_LEN +#define MAC_LEN 6 +#endif +#define DEV_NAME_MAX 16 + +typedef struct { + in_addr_t ip; /* IP in network byte order */ + unsigned hw_type; + unsigned flags; + uint8_t mac[MAC_LEN]; + char dev[DEV_NAME_MAX]; +} arp_entry_t; + +extern int arp_table_get(arp_entry_t *arp_table, int *arp_entries); +extern int arp_table_find_by_ip(arp_entry_t *arp_table, int arp_entries, in_addr_t ip); +extern void arp_table_dump(int priority, const arp_entry_t *arp_table, int arp_entries); + +#ifdef __cplusplus +}; +#endif + +#endif /* ARP_TABLE_H */ diff --git a/flightcode/util/file_util.c b/flightcode/util/file_util.c new file mode 100644 index 0000000..bfce284 --- /dev/null +++ b/flightcode/util/file_util.c @@ -0,0 +1,38 @@ + +#include +#include +#include +#include +#include "file_util.h" + +/* +* file_exists - determine whether a file exists +* +* Return nonzero (true) if file exists, zero (false) if file does not exist. +*/ +int file_exists(const char *filename) +{ + struct stat file_stat; + return (stat(filename, &file_stat) == 0); +} + +/* +* file_touch - create file if it does not exist +* +* Method of doing this (the mode flags) is per 'man touch'. +*/ +int file_touch(const char *filename) +{ + int fd; + mode_t mode; + + mode = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH; + + fd = creat(filename, mode); + if (fd < 0) { + return 0; + } else { + close(fd); + return 1; + } +} diff --git a/flightcode/util/file_util.h b/flightcode/util/file_util.h new file mode 100644 index 0000000..20f1789 --- /dev/null +++ b/flightcode/util/file_util.h @@ -0,0 +1,15 @@ +#ifndef FILE_UTIL_H +#define FILE_UTIL_H + +#ifdef __cplusplus +extern "C" { +#endif + +extern int file_exists(const char *filename); +extern int file_touch(const char *filename); + +#ifdef __cplusplus +}; +#endif + +#endif /* FILE_UTIL_H */ diff --git a/flightcode/util/hostapd_ctrl.c b/flightcode/util/hostapd_ctrl.c new file mode 100644 index 0000000..17a6ee0 --- /dev/null +++ b/flightcode/util/hostapd_ctrl.c @@ -0,0 +1,192 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "util.h" +#include "hostapd_ctrl.h" + +#define IF_NAME_MAX 32 + +#define HOSTAPD_REPLY_TIMEOUT_US 1000000 + +typedef struct { + char if_name[IF_NAME_MAX]; + struct sockaddr_un sa_loc; + struct sockaddr_un sa_dst; + int fd; +} hostapd_ctrl_info_t; + +/* +* hostapd_ctrl_new - create a hostapd control connection +* +* Returns non-NULL handle on success, or NULL on error. +*/ +void *hostapd_ctrl_new(const char *if_name) +{ + hostapd_ctrl_info_t *ctrl; + struct timeval timeout; + + ctrl = (hostapd_ctrl_info_t *)malloc(sizeof(hostapd_ctrl_info_t)); + if (ctrl == NULL) + goto hostapd_ctrl_new_error_1; + + memset(ctrl, 0, sizeof(*ctrl)); + + strncpy(ctrl->if_name, if_name, IF_NAME_MAX - 1); + + ctrl->fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (ctrl->fd == -1) + goto hostapd_ctrl_new_error_2; + + ctrl->sa_loc.sun_family = AF_UNIX; + sprintf(ctrl->sa_loc.sun_path, "/tmp/hostapd_ctrl.%ld", syscall(SYS_gettid)); + if (bind(ctrl->fd, (struct sockaddr *)&(ctrl->sa_loc), sizeof(ctrl->sa_loc)) != 0) + goto hostapd_ctrl_new_error_3; + + timeout.tv_sec = HOSTAPD_REPLY_TIMEOUT_US / 1000000; + timeout.tv_usec = HOSTAPD_REPLY_TIMEOUT_US % 1000000; + if (setsockopt(ctrl->fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout)) != 0) + goto hostapd_ctrl_new_error_3; + + ctrl->sa_dst.sun_family = AF_UNIX; + if (snprintf(ctrl->sa_dst.sun_path, sizeof(ctrl->sa_dst.sun_path), "/var/run/hostapd/%s", + if_name) >= sizeof(ctrl->sa_dst.sun_path)) + goto hostapd_ctrl_new_error_3; + + return (void *)ctrl; + +hostapd_ctrl_new_error_3: + close(ctrl->fd); +hostapd_ctrl_new_error_2: + memset(ctrl, 0, sizeof(*ctrl)); + free(ctrl); +hostapd_ctrl_new_error_1: + return NULL; + +} /* hostapd_ctrl_new */ + +/* +* hostapd_ctrl_delete - delete a hostapd control connection +*/ +int hostapd_ctrl_delete(void *handle) +{ + hostapd_ctrl_info_t *ctrl = (hostapd_ctrl_info_t *)handle; + + close(ctrl->fd); + unlink(ctrl->sa_loc.sun_path); + memset(ctrl, 0, sizeof(*ctrl)); + free(ctrl); + + return 0; + +} /* hostapd_ctrl_delete */ + +/* +* hostapd_ctrl_get_stations - query hostapd for all attached stations +* +* On entry, station_info points to a caller-supplied table of *station_entries +* hostapd_station_info_t structures. This function fills in the table, up to +* *station_entries entries. On return, *station_entries is the number of +* stations associated. If there are more stations than will fit in the table, +* the table is filled up, then *station_entries is set to the actual number of +* stations. +* +* Returns 0 on success, or nonzero on error. If an error is returned, neither +* the contents of station_info nor *station_entries is valid. +*/ +int hostapd_ctrl_get_stations(const void *handle, hostapd_station_info_t *station_info, + int *station_entries) +{ + const hostapd_ctrl_info_t *ctrl = (const hostapd_ctrl_info_t *)handle; + char request[64]; + int request_len; + ssize_t status; + char reply[1024]; + char *saveptr; + char *mac; + int mac_len; + int entries = 0; + + strcpy(request, "STA-FIRST"); + request_len = strlen(request); + status = sendto(ctrl->fd, request, request_len, 0, (struct sockaddr *)&(ctrl->sa_dst), + sizeof(ctrl->sa_dst)); + if (status != request_len) { + perror("sendto"); + return -1; + } + + do { + + memset(reply, 0, sizeof(reply)); + status = recv(ctrl->fd, reply, sizeof(reply), 0); + if (status < 0) { + perror("recv"); + return -1; + } else if (status == 0) { + /* normal return; get this after the last station */ + break; + } + + /*printf("%d bytes: \"%s\"\n", status, reply);*/ + + /* get first token (mac) */ + mac = strtok_r(reply, "\n", &saveptr); + if (mac == NULL) { + printf("ERROR: getting mac from reply\n"); + return -1; + } + mac_len = strlen(mac); + if (mac_len != 17) { + printf("ERROR: mac is strange length\n"); + return -1; + } + + /* only save info if there is room; otherwise, we are just counting + stations so we can set station_entries */ + if (entries < *station_entries) { + + if (mac_aton(mac, station_info[entries].mac) == NULL) { + printf("ERROR: parsing mac\n"); + return -1; + } + } + + entries++; + + sprintf(request, "STA-NEXT %s", mac); + request_len = strlen(request); + status = sendto(ctrl->fd, request, request_len, 0, (struct sockaddr *)&(ctrl->sa_dst), + sizeof(ctrl->sa_dst)); + if (status != request_len) { + perror("sendto"); + return -1; + } + + } while (1); + + *station_entries = entries; + + return 0; + +} /* hostapd_ctrl_get_stations */ + +int hostapd_ctrl_find_by_mac(const hostapd_station_info_t *station_info, int station_entries, + const uint8_t *mac) +{ + int i; + + for (i = 0; i < station_entries; i++) + if (memcmp(mac, station_info[i].mac, MAC_LEN) == 0) + return i; + + /* not found */ + return -1; + +} /* hostapd_ctrl_find_by_mac */ diff --git a/flightcode/util/hostapd_ctrl.h b/flightcode/util/hostapd_ctrl.h new file mode 100644 index 0000000..d9762a0 --- /dev/null +++ b/flightcode/util/hostapd_ctrl.h @@ -0,0 +1,29 @@ +#ifndef HOSTAPD_CTRL_H +#define HOSTAPD_CTRL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAC_LEN +#define MAC_LEN 6 +#endif + +extern void *hostapd_ctrl_new(const char *ifname); +extern int hostapd_ctrl_delete(void *handle); + +typedef struct { + uint8_t mac[MAC_LEN]; + /* more fields are available, but not parsed */ +} hostapd_station_info_t; + +extern int hostapd_ctrl_get_stations(const void *handle, hostapd_station_info_t *station_info, + int *station_entries); +extern int hostapd_ctrl_find_by_mac(const hostapd_station_info_t *station_info, int station_entries, + const uint8_t *mac); + +#ifdef __cplusplus +}; +#endif + +#endif /* HOSTAPD_CTRL_H */ diff --git a/flightcode/util/link_packet.h b/flightcode/util/link_packet.h new file mode 100644 index 0000000..74bceb4 --- /dev/null +++ b/flightcode/util/link_packet.h @@ -0,0 +1,24 @@ +#ifndef LINK_PACKET_H +#define LINK_PACKET_H + +#include + +struct LinkPacket { + static const int MAX_PAYLOAD = 1444; /* MTU minus the header */ + + uint64_t tf_recv_us; + uint64_t tf_send_us; + uint64_t tc_recv_us; + uint64_t tc_send_us; + uint64_t stm_recv_us; + uint32_t seq; + uint32_t data1; + uint32_t data2; + uint32_t data3; + uint8_t payload[MAX_PAYLOAD]; + + // everything but the payload + static const int HDR_LEN = 56; +}; + +#endif // LINK_PACKET_H diff --git a/flightcode/util/main.c b/flightcode/util/main.c new file mode 100644 index 0000000..bb9a1dc --- /dev/null +++ b/flightcode/util/main.c @@ -0,0 +1,23 @@ +#include +#include +#include "util_test.h" +#include "syslog_test.h" + +int main(int argc, char *argv[]) +{ + + fprintf(stderr, "util_test..."); + if (util_test() == 0) + fprintf(stderr, "pass\n"); + else + fprintf(stderr, "FAIL\n"); + + fprintf(stderr, "syslog_test..."); + if (syslog_test() == 0) + fprintf(stderr, "pass\n"); + else + fprintf(stderr, "FAIL\n"); + + return 0; + +} // main diff --git a/flightcode/util/mutex.c b/flightcode/util/mutex.c new file mode 100644 index 0000000..72df28c --- /dev/null +++ b/flightcode/util/mutex.c @@ -0,0 +1,194 @@ + +#include +#include +#include +#include "util.h" +#include "mutex.h" + +/* +* mutex_type_str - return string name of mutex type attribute (debug) +*/ +static const char *mutex_type_str(int type) +{ + switch (type) { + case PTHREAD_MUTEX_NORMAL: + return "PTHREAD_MUTEX_NORMAL"; + case PTHREAD_MUTEX_ERRORCHECK: + return "PTHREAD_MUTEX_ERRORCHECK"; + case PTHREAD_MUTEX_RECURSIVE: + return "PTHREAD_MUTEX_RECURSIVE"; + /* PTHREAD_MUTEX_DEFAULT = PTHREAD_MUTEX_NORMAL */ + default: + return "UNKNOWN"; + } +} + +/* +* mutex_protocol_str - return string name of mutex protocol attribute (debug) +*/ +static const char *mutex_protocol_str(int protocol) +{ + switch (protocol) { + case PTHREAD_PRIO_NONE: + return "PTHREAD_PRIO_NONE"; + case PTHREAD_PRIO_INHERIT: + return "PTHREAD_PRIO_INHERIT"; + case PTHREAD_PRIO_PROTECT: + return "PTHREAD_PRIO_PROTECT"; + default: + return "UNKNOWN"; + } +} + +/* +* mutex_pshared_str - return string name of mutex pshared attribute (debug) +*/ +static const char *mutex_pshared_str(int pshared) +{ + switch (pshared) { + case PTHREAD_PROCESS_SHARED: + return "PTHREAD_PROCESS_SHARED"; + case PTHREAD_PROCESS_PRIVATE: + return "PTHREAD_PROCESS_PRIVATE"; + default: + return "UNKNOWN"; + } +} + +/* +* mutex_attr_show - print mutex attributes to stdout (debug) +*/ +void mutex_attr_show(const pthread_mutexattr_t *attr) +{ + int rc; + int val; + + printf("mutex attributes @ %10p:\n", attr); + + rc = pthread_mutexattr_gettype(attr, &val); + if (rc != 0) + printf("ERROR %d returned from pthread_mutexattr_gettype\n", rc); + else + printf("type=%s\n", mutex_type_str(val)); + + rc = pthread_mutexattr_getprioceiling(attr, &val); + if (rc != 0) + printf("ERROR %d returned from pthread_mutexattr_getprioceiling\n", rc); + else + printf("prio_ceiling=%d\n", val); + + rc = pthread_mutexattr_getprotocol(attr, &val); + if (rc != 0) + printf("ERROR %d returned from pthread_mutexattr_getprotocol\n", rc); + else + printf("protocol=%s\n", mutex_protocol_str(val)); + + rc = pthread_mutexattr_getpshared(attr, &val); + if (rc != 0) + printf("ERROR %d returned from pthread_mutexattr_getpshared\n", rc); + else + printf("pshared=%s\n", mutex_pshared_str(val)); +} + +/* +* mutex_init - initialize a mutex for use with realtime threads +* +* Default mutex attributes have a non-realtime priority ceiling, and do not +* have priority inversion enabled. This changes the defaults so the priority +* ceiling is the highest priority, priority inversion is enabled, and error +* checking it turned on. +* +* Attribute Default Setting Here +* --------------- -------------------------- -------------------------- +* type PTHREAD_MUTEX_NORMAL PTHREAD_MUTEX_ERRORCHECK +* prio_ceiling 1 99 +* protocol PTHREAD_PRIO_NONE PTHREAD_PRIO_INHERIT +* pshared PTHREAD_PROCESS_PRIVATE PTHREAD_PROCESS_PRIVATE +* +* The priority specified for prio_ceiling is the SCHED_FIFO priority, i.e. +* 1 is lowest, 99 is highest. +*/ +int mutex_init(pthread_mutex_t *mutex) +{ + pthread_mutexattr_t attr; + int rc; + + /* set up attributes */ + + rc = pthread_mutexattr_init(&attr); + if (rc != 0) + return rc; + + /* mutex_attr_show(&attr); */ + + rc = pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_ERRORCHECK); + if (rc != 0) + return rc; + + rc = pthread_mutexattr_setprioceiling(&attr, 99); + if (rc != 0) + return rc; + + rc = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); + if (rc != 0) + return rc; + + rc = pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_PRIVATE); + if (rc != 0) + return rc; + + /* mutex_attr_show(&attr); */ + + rc = pthread_mutex_init(mutex, &attr); + if (rc != 0) + return rc; + + return 0; + +} /* mutex_init */ + +/* +* mutex_lock - lock mutex, keeping track of maximum block time +* +* If max_blocked_us is not NULL, measure how long it took to lock the +* mutex (how long we were blocked). Then, if the new blocked time was +* longer, update max_blocked_us; if the new blocked time was not +* longer, do not change max_blocked_us. +* +* To ignore how long the lock took, set max_blocked_us to NULL. +* +* To measure how long the lock took without regard to whether it is +* a new maximum, initialize *max_blocked_us to 0. +*/ +int mutex_lock(pthread_mutex_t *mutex, uint64_t *max_blocked_us) +{ + int rc; + uint64_t start_us; + uint64_t blocked_us; + + if (max_blocked_us != NULL) + start_us = clock_gettime_us(CLOCK_MONOTONIC); + + rc = pthread_mutex_lock(mutex); + + if (rc == 0 && max_blocked_us != NULL) { + blocked_us = clock_gettime_us(CLOCK_MONOTONIC) - start_us; + if (*max_blocked_us < blocked_us) + *max_blocked_us = blocked_us; + } + + return rc; + +} /* mutex_lock */ + +/* +* mutex_unlock - unlock mutex +* +* Provided for symmetry with mutex_lock. +*/ +int mutex_unlock(pthread_mutex_t *mutex) +{ + + return pthread_mutex_unlock(mutex); + +} /* mutex_unlock */ diff --git a/flightcode/util/mutex.h b/flightcode/util/mutex.h new file mode 100644 index 0000000..bce3892 --- /dev/null +++ b/flightcode/util/mutex.h @@ -0,0 +1,20 @@ +#ifndef MUTEX_H +#define MUTEX_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +extern void mutex_attr_show(const pthread_mutexattr_t *attr); +extern int mutex_init(pthread_mutex_t *mutex); +extern int mutex_lock(pthread_mutex_t *mutex, uint64_t *max_blocked_us); +extern int mutex_unlock(pthread_mutex_t *mutex); + +#ifdef __cplusplus +}; +#endif + +#endif /* MUTEX_H */ diff --git a/flightcode/util/net_stats.c b/flightcode/util/net_stats.c new file mode 100644 index 0000000..40cea20 --- /dev/null +++ b/flightcode/util/net_stats.c @@ -0,0 +1,141 @@ + +#include +#include +#include +#include +#include "net_stats.h" + +/* The udp rx queue size is reported in multiples of some kernel-internal + * buffer. We want the number of packets queued, which is presumably the + * number of buffers. However, if we ever see a rx queue value that is not + * a multiple of this, start reporting the raw number. */ +#define UDP_RX_QUEUE_BUFSIZE 2496 +int udp_rx_queue_bufsize_correct = 1; /* assume correct */ + +/* +* udp_info_get - get info about a udp port +* +* Read /proc/net/udp and fill in udp_info with status of a particular port. +* At the moment we only care about the rx queue size, so that is the only +* field in udp_info_t, and the only thing parsed from /proc/net/udp for the +* port. +* +* /proc/net/udp: +* sl local_address rem_address st tx_queue rx_queue tr tm->when retrnsmt uid timeout inode +* ref pointer drops +* 53: 00000000:0035 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 5842 2 +* 86b0c500 0 +* 67: 00000000:0043 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 5837 2 +* 86b0c280 0 +* 136: 00000000:A288 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 31927 +* 2 86b0c780 0 +* 141: 00000000:138D 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 31880 +* 2 86b0cc80 0 +* 205: 00000000:15CD 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 31982 +* 2 86b0cf00 0 +* 209: 00000000:CFD1 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 31855 +* 2 86b0d180 0 +* 224: 00000000:38E0 00000000:0000 07 00000000:00000000 00:00000000 00000000 0 0 31870 +* 2 86b0ca00 0 +* +* Returns 0 on success, nonzero on error. +*/ +int udp_info_get(unsigned port, udp_info_t *udp_info) +{ + int status = -1; + FILE *fp; + char linebuf[200]; + + memset(udp_info, 0, sizeof(*udp_info)); + + fp = fopen("/proc/net/udp", "r"); + if (fp == NULL) + return -1; + + while (fgets(linebuf, sizeof(linebuf), fp) != NULL) { + char *saveptr; + char *token; + unsigned token_len; + char *endptr; + unsigned token_val; + + /* get first token (sl, "53") */ + token = strtok_r(linebuf, " :", &saveptr); + if (token == NULL) + continue; /* error getting first token in line */ + token_len = strlen(token); + if (token_len == 0) + continue; /* odd... */ + if (!isdigit(token[0])) + continue; /* first line, token is "sl" */ + /* token not used */ + + /* get next token (local_address/IP, "00000000") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + /* token not used */ + + /* get third token local_address/port in hex */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + endptr = NULL; + token_val = strtol(token, &endptr, 16); + if (*endptr != '\0') + continue; /* error converting token from hex to decimal */ + if (token_val != port) + continue; /* not the port we're looking for */ + + /* get next token (remote_address/IP, "00000000") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + /* token not used */ + + /* get next token (remote_address/port, "0000") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + /* token not used */ + + /* get next token (st, "07") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + /* token not used */ + + /* get next token (tx_queue, "00000000") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + /* token not used */ + + /* get next token (rx_queue, "00000000") */ + token = strtok_r(NULL, " :", &saveptr); + if (token == NULL) + continue; /* error getting next token in line */ + endptr = NULL; + token_val = strtol(token, &endptr, 16); + if (*endptr != '\0') + continue; /* error converting token from hex to decimal */ + /* verify our assumed rx queue bufsize is correct */ + if ((token_val % UDP_RX_QUEUE_BUFSIZE) != 0) + udp_rx_queue_bufsize_correct = 0; + /* if correct, adjust the queue depth we are reporting */ + if (udp_rx_queue_bufsize_correct) + token_val = token_val / UDP_RX_QUEUE_BUFSIZE; + /* report queue depth */ + udp_info->rx_queue = token_val; + + /* not interested in any more fields; we are done */ + status = 0; + break; + + } /* while (fgets...) */ + + fclose(fp); + + return status; + +} /* udp_info_get */ diff --git a/flightcode/util/net_stats.h b/flightcode/util/net_stats.h new file mode 100644 index 0000000..391de97 --- /dev/null +++ b/flightcode/util/net_stats.h @@ -0,0 +1,18 @@ +#ifndef NET_STATS_H +#define NET_STATS_H + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + unsigned rx_queue; +} udp_info_t; + +extern int udp_info_get(unsigned port, udp_info_t *info); + +#ifdef __cplusplus +}; +#endif + +#endif /* NET_STATS_H */ diff --git a/flightcode/util/net_wmm.h b/flightcode/util/net_wmm.h new file mode 100644 index 0000000..3398c11 --- /dev/null +++ b/flightcode/util/net_wmm.h @@ -0,0 +1,18 @@ +#ifndef NET_WMM_H +#define NET_WMM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define IP_TOS_DEFAULT 0 /* default (best effort) */ +#define IP_TOS_BK (1 << 5) /* background, lowest */ +#define IP_TOS_BE (3 << 5) /* best effort, default */ +#define IP_TOS_VI (5 << 5) /* video */ +#define IP_TOS_VO (7 << 5) /* voice, highest */ + +#ifdef __cplusplus +}; +#endif + +#endif /* NET_WMM_H */ diff --git a/flightcode/util/proc_table.c b/flightcode/util/proc_table.c new file mode 100644 index 0000000..9c7e5d4 --- /dev/null +++ b/flightcode/util/proc_table.c @@ -0,0 +1,317 @@ + +#include +#include + +#include +#include +#include +#include +#include +#include "util.h" +#include "proc_table.h" + +/* +* proc_table_get - get proc table +* +* Read +* /proc//stat +* /proc//task//stat +* and fill in proc_table. +* +* We don't get a true snapshot, since things under /proc can change while +* reading through them. Each open/read needs to handle sudden disappearance. +* +* 'man proc' describes the contents of the stat files. Example contents of +* stat files below have line breaks inserted for readability. +* +* /proc/861/stat: +* +* 861 (telem_forwarder) S 1 861 861 0 -1 4194560 427 0 4 0 217 2685 0 0 20 +* 0 4 0 10613 30613504 328 4294967295 32768 84056 2125479472 2125478440 +* 1993544836 0 0 0 0 4294967295 0 0 17 0 0 0 0 0 0 118784 119528 25137152 +* 2125479709 2125479725 2125479725 2125479907 0 +* +* pid=861 utime=217 stime=2685 +* +* /proc/861/task/@/stat: +* +* 861 (telem_forwarder) S 1 861 861 0 -1 4194560 408 0 4 0 0 13 0 0 20 +* 0 4 0 10613 30613504 328 4294967295 32768 84056 2125479472 2125478440 +* 1993544836 0 0 0 0 2147779492 0 0 17 0 0 0 0 0 0 118784 119528 25137152 +* 2125479709 2125479725 2125479725 2125479907 0 +* +* pid=861 utime=0 stime=13 +* +* 878 (telem_forwarder) S 1 861 861 0 -1 4194624 4 0 0 0 193 2595 0 0 -40 +* 0 4 0 10659 30613504 328 4294967295 32768 84056 2125479472 1992428328 +* 1993700708 0 0 0 0 2148340764 0 0 -1 0 39 1 0 0 0 118784 119528 25137152 +* 2125479709 2125479725 2125479725 2125479907 0 +* +* pid=878 utime=193 stime=2595 +* +* 879 (telem_forwarder) S 1 861 861 0 -1 4194368 5 0 0 0 14 49 0 0 -41 +* 0 4 0 10659 30613504 328 4294967295 32768 84056 2125479472 1984040064 +* 1995229292 0 0 0 0 2151581412 0 0 -1 0 40 1 0 0 0 118784 119528 25137152 +* 2125479709 2125479725 2125479725 2125479907 0 +* +* pid=879 utime=14 stime=49 +* +* 880 (telem_forwarder) S 1 861 861 0 -1 4194368 10 0 0 0 12 23 0 0 -4 +* 0 4 0 10659 30613504 328 4294967295 32768 84056 2125479472 1975655240 +* 1993544836 0 0 0 0 2147779492 0 0 -1 0 3 1 0 0 0 118784 119528 25137152 +* 2125479709 2125479725 2125479725 2125479907 0 +* +* pid=880 utime=12 stime=23 +* +* Oddly, the utimes and stimes of the tasks don't sum to the utime and stime +* of the overall process; from the above, the task sums are: +* utime=219 stime=2680 +* compared to the process's: +* utime=217 stime=2685 +* +* On success, proc_table is filled in with proc entries, proc_entries is set +* to the number of entries, and 0 is returned. On error, nonzero is returned. +*/ +int proc_table_get(proc_entry_t *proc_table, int *proc_entries) +{ + int status = 0; + int entries; + DIR *dp1; + struct dirent *de1; + DIR *dp2; + struct dirent *de2; + int pid; + int tid; + char *endptr; + char tasks_dir[32]; + char file_name[48]; + FILE *fp; + int match; + + if (proc_table == NULL || proc_entries == NULL || *proc_entries == 0) { + fprintf(stderr, "proc_table_get: invalid arguments\n"); + return -1; + } + + memset(proc_table, 0, *proc_entries * sizeof(proc_entry_t)); + entries = 0; + + dp1 = opendir("/proc"); + if (dp1 == NULL) { + fprintf(stderr, "error opening directory /proc\n"); + return -1; + } + + /* loop through /proc// until the table fills up or we read all */ + while ((entries < *proc_entries) && ((de1 = readdir(dp1)) != NULL)) { + /* quick check: ignore non-directory entries */ + if (de1->d_type != DT_DIR) + continue; + /* it's a pid if it can be converted to an integer */ + pid = strtol(de1->d_name, &endptr, 10); + if (*endptr != '\0') + continue; + /* loop through process's tasks */ + sprintf(tasks_dir, "/proc/%d/task", pid); + dp2 = opendir(tasks_dir); + if (dp2 == NULL) { + /* this happens if the process disappears between the time we find + /proc// and the time we try to open /proc//task/ */ + /*fprintf(stderr, "error opening directory %s\n", tasks_dir);*/ + continue; + } + + /* loop over all /proc//task// */ + while ((entries < *proc_entries) && ((de2 = readdir(dp2)) != NULL)) { + /* quick check: ignore non-directory entries */ + if (de2->d_type != DT_DIR) + continue; + /* it's a tid if it can be converted to an integer */ + tid = strtol(de2->d_name, &endptr, 10); + if (*endptr != '\0') + continue; + /* read task's stat file */ + sprintf(file_name, "/proc/%d/task/%d/stat", pid, tid); + fp = fopen(file_name, "r"); + if (fp == NULL) { + /* this has not been observed, but presumably could happen if + the process or task disappears between the time we find + /proc// or /proc//task/, and the time we try + to open /proc//task//stat */ + /*fprintf(stderr, "error opening file %s\n", file_name);*/ + continue; + } + + /* + * Format as specified by 'man proc': + * %d (%s) %c %d %d %d %d %d %u %lu + * %lu %lu %lu %lu %lu %ld %ld %ld %ld %ld + * %ld %llu %lu %ld %lu %lu %lu %lu %lu %lu + * %lu %lu %lu %lu %lu %lu %lu %d %d %u + * %u %llu %lu %ld %lu %lu %lu %lu %lu %lu + * %lu %d + */ + match = fscanf(fp, "%d %31s %*c %*d %*d %*d %*d %*d %*u %*u " + "%*u %*u %*u %lu %lu %*d %*d %ld %ld %*d " + "%*d %*u %*u %*d %*u %*u %*u %*u %*u %*u " + "%*u %*u %*u %*u %*u %*u %*u %*d %*d %*u " + "%*u %*u %*u %*d %*u %*u %*u %*u %*u %*u " + "%*u %*d", + &proc_table->pid, proc_table->comm, &proc_table->utime, + &proc_table->stime, &proc_table->priority, &proc_table->nice); + if (match == 6) { + proc_table++; + entries++; + } else { + /* fscanf fails to match for a process name with spaces (!), + e.g. "ci otg thread". It may also fail if the process or + task disappears (that has not been observed). */ + /*fprintf(stderr, "error parsing line for %s (matched %d)\n", + file_name, match);*/ + } + fclose(fp); + + } /* while (de2...) */ + + closedir(dp2); + + } /* while (de1...) */ + + closedir(dp1); + + *proc_entries = entries; + + return status; + +} /* proc_table_get */ + +/* +* proc_table_diff - find diffs between two proc_tables +* +* For each pid that appears in both proc tables, create an output entry +* containing the difference between the corresponding utime and stime fields. +* +* Output table cannot be the same as either input table. +* +* Differences are computed as (table 2 value) - (table 1 value). +*/ +int proc_table_diff(proc_entry_t *proc_table_1, int proc_entries_1, proc_entry_t *proc_table_2, + int proc_entries_2, proc_entry_t *proc_table_3, int *proc_entries_3) +{ + int entry1; + int entry2; + int entry3; + int pid; + + entry3 = 0; + + /* proc_entries_1 or proc_entries_2 can be zero */ + if (proc_table_1 == NULL || proc_table_2 == NULL || proc_table_3 == NULL || + proc_table_3 == proc_table_1 || proc_table_3 == proc_table_2 || proc_entries_3 == NULL || + *proc_entries_3 == 0) { + fprintf(stderr, "proc_table_diff: invalid arguments\n"); + return -1; + } + + memset(proc_table_3, 0, *proc_entries_3 * sizeof(proc_entry_t)); + + /* loop through proc_table_1 */ + for (entry1 = 0; entry1 < proc_entries_1; entry1++) { + pid = proc_table_1[entry1].pid; + + /* see if there is a matching entry in proc_table_2 */ + for (entry2 = 0; entry2 < proc_entries_2; entry2++) + if (proc_table_2[entry2].pid == pid) + break; /* match! */ + + /* was there a match? */ + if (proc_table_2[entry2].pid != pid) + continue; /* no */ + + /* proc_table_1[entry1] and proc_table_2[entry2] are the same pid */ + + proc_table_3[entry3].pid = pid; + memcpy(proc_table_3[entry3].comm, proc_table_1[entry1].comm, COMM_MAX); + proc_table_3[entry3].utime = proc_table_2[entry2].utime - proc_table_1[entry1].utime; + proc_table_3[entry3].stime = proc_table_2[entry2].stime - proc_table_1[entry1].stime; + /* all other fields are left zero */ + + entry3++; + + if (entry3 >= *proc_entries_3) + break; + + } /* for (entry1...) */ + + *proc_entries_3 = entry3; + + return 0; + +} /* proc_table_diff */ + +/* +* proc_table_top - sort proc table by top CPU users +* +* The goal is to get this right the first time - no instabilities. +* +* We have around 70 entries, and most of those (80-90%) have zero CPU usage. +* An insertion sort is used: go through the array, and for each nonzero entry, +* insert it at its sorted position. This should result in an insertion for +* each entry with nonzero CPU usage; the zero entries can be skipped quickly +* and will end up at the bottom. +*/ +void proc_table_top(proc_entry_t *proc_table, int proc_entries) +{ + int i, j; + long unsigned sum1; + long unsigned sum2; + proc_entry_t p; + + /* loop over all entries */ + for (i = 0; i < proc_entries; i++) { + sum1 = proc_table[i].utime + proc_table[i].stime; + /* skip zero entries */ + if (sum1 == 0) + continue; + /* All entries above this one are already sorted. Insert this one at + the sorted position (above its current position). If it should + stay where it is, then we'll go through the j loop without doing + the insertion. */ + for (j = 0; j < i; j++) { + sum2 = proc_table[j].utime + proc_table[j].stime; + if (sum2 < sum1) { + /* insert i at j */ + + /* save entry at i */ + p = proc_table[i]; /* structure copy */ + + /* move down, leaving empty slot at j, overwriting slot at i */ + memmove(&proc_table[j + 1], &proc_table[j], (i - j) * sizeof(proc_entry_t)); + + /* copy saved entry to j */ + proc_table[j] = p; /* structure copy */ + + break; /* for j */ + } + + } /* for (j...) */ + + } /* for (i...) */ + +} /* proc_table_top */ + +/* +* proc_table_dump - print proc table to stdout +*/ +void proc_table_dump(const proc_entry_t *proc_table, int proc_entries) +{ + int i; + + printf("proc_table (%d entries):", proc_entries); + + for (i = 0; i < proc_entries; i++) + printf("%6d %-20s %6lu %6lu %4ld %4ld\n", proc_table[i].pid, proc_table[i].comm, + proc_table[i].utime, proc_table[i].stime, proc_table[i].priority, + proc_table[i].nice); + +} /* proc_table_dump */ diff --git a/flightcode/util/proc_table.h b/flightcode/util/proc_table.h new file mode 100644 index 0000000..283772f --- /dev/null +++ b/flightcode/util/proc_table.h @@ -0,0 +1,78 @@ +#ifndef PROC_TABLE_H +#define PROC_TABLE_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + int pid; /* (1) pid %d */ +#define COMM_MAX 32 + char comm[COMM_MAX]; /* (2) comm %s */ + /* (3) state %c */ + /* (4) ppid %d */ + /* (5) pgrp %d */ + /* (6) session %d */ + /* (7) tty_nr %d */ + /* (8) tpgid %d */ + /* (9) flags %u */ + /* (10) minflt %lu */ + /* (11) cminflt %lu */ + /* (12) majflt %lu */ + /* (13) cmajflt %lu */ + long unsigned utime; /* (14) utime %lu */ + long unsigned stime; /* (15) stime %lu */ + /* (16) cutime %ld */ + /* (17) cstime %ld */ + long int priority; /* (18) priority %ld */ + long int nice; /* (19) nice %ld */ + /* (20) num_threads %ld */ + /* (21) itrealvalue %ld */ + /* (22) starttime %llu */ + /* (23) vsize %lu */ + /* (24) rss %ld */ + /* (25) rsslim %lu */ + /* (26) startcode %lu */ + /* (27) endcode %lu */ + /* (28) startstack %lu */ + /* (29) kstkesp %lu */ + /* (30) kstkeip %lu */ + /* (31) signal %lu */ + /* (32) blocked %lu */ + /* (33) sigignore %lu */ + /* (34) sigcatch %lu */ + /* (35) wchan %lu */ + /* (36) nswap %lu */ + /* (37) cnswap %lu */ + /* (38) exit_signal %d */ + /* (39) processor %d */ + /* (40) rt_priority %u */ + /* (41) policy %u */ + /* (42) delayacct_blkio_ticks %llu */ + /* (43) guest_time %lu */ + /* (44) cguest_time %ld */ + /* (45) start_data %lu */ + /* (46) end_data %lu */ + /* (47) start_brk %lu */ + /* (48) arg_start %lu */ + /* (49) arg_end %lu */ + /* (50) env_start %lu */ + /* (51) env_end %lu */ + /* (52) exit_code %d */ + +} proc_entry_t; + +extern int proc_table_get(proc_entry_t *proc_table, int *proc_entries); +extern int proc_table_diff(proc_entry_t *proc_table_1, int proc_entries_1, + proc_entry_t *proc_table_2, int proc_entries_2, + proc_entry_t *proc_table_3, int *proc_entries_3); +extern void proc_table_top(proc_entry_t *proc_table, int proc_entries); +extern void proc_table_dump(const proc_entry_t *proc_table, int proc_entries); + +#ifdef __cplusplus +}; +#endif + +#endif /* PROC_TABLE_H */ diff --git a/flightcode/util/syslog.c b/flightcode/util/syslog.c new file mode 100644 index 0000000..0b5f88a --- /dev/null +++ b/flightcode/util/syslog.c @@ -0,0 +1,274 @@ + +#include "util.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// can build it in blocking mode for testing +#undef INCLUDE_BLOCKING + +// can show some of the long block times with this enabled +#undef INCLUDE_HISTOGRAM + +#ifndef SYSLOG_SOCK_NAME +#define SYSLOG_SOCK_NAME "/dev/log" +#endif + +#ifndef SYSLOG_MSG_MAX +#define SYSLOG_MSG_MAX 512 +#endif + +static int log_fd = -1; +static const char *log_ident = "-"; +static int log_facility = LOG_USER; +static int log_dropped = 0; +static int log_options = 0; +#define LOG_MASK_ALL 0xff +static int log_mask = LOG_MASK_ALL; + +#ifdef INCLUDE_HISTOGRAM +#define hist_bins 100 +static int hist[hist_bins]; +static unsigned hist_dump_interval_us = 60 * 1000000; +#endif + +int setlogmask(int new_mask) +{ + int old_mask = log_mask; + + if (new_mask != 0 && (new_mask & LOG_MASK_ALL) == 0) + log_mask = new_mask; + + return old_mask; + +} // setlogmask + +void openlog(const char *ident, int options, int facility) +{ + + int fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (fd < 0) { + perror("openlog: socket"); + return; + } + +#ifndef INCLUDE_BLOCKING + if (fcntl(fd, F_SETFL, O_NONBLOCK) != 0) { + perror("openlog: fcntl"); + close(fd); + return; + } +#endif + + struct sockaddr_un sa_un; + memset(&sa_un, 0, sizeof(sa_un)); + sa_un.sun_family = AF_UNIX; + strncpy(sa_un.sun_path, SYSLOG_SOCK_NAME, sizeof(sa_un.sun_path) - 1); + if (connect(fd, (struct sockaddr *)&sa_un, sizeof(sa_un)) != 0) { + perror("openlog: connect"); + close(fd); + return; + } + + log_facility = facility; + log_ident = ident; + log_options = options; + + log_fd = fd; + +} // openlog + +void closelog(void) +{ + + int fd = log_fd; + + log_fd = -1; + log_facility = LOG_USER; + log_ident = "-"; + log_options = 0; + log_dropped = 0; + + close(fd); + +} // closelog + +// format as expected by busybox's syslogd +// return 0 on success, nonzero on error +static int vsyslog2(int sev, const char *fmt, va_list ap) +{ + + if (log_fd < 0 || sev < 0 || sev > 7 || fmt == NULL) + return 1; + + if (!(LOG_MASK(LOG_PRI(sev)) & log_mask)) + return 0; + + uint64_t now_us = clock_gettime_us(CLOCK_REALTIME); + time_t s = now_us / 1000000; + struct tm now_tm; + localtime_r(&s, &now_tm); + + char msg[SYSLOG_MSG_MAX]; + char *p = msg; + int remain = sizeof(msg) - 1; // never overwrite the last '\0' + int n; + + memset(msg, 0, sizeof(msg)); + + // Notes on error checking: snprintf(str, size, fmt, ...) returns 'size' + // or more if there is not enough room. We detect that by updating the + // remaining room, then not writing any more if that becomes nonpositive. + // To guarantee the resulting string (even if truncated due to overflow) + // is always '\0' terminated, we started with it zeroed and initialized + // remaining room to one less than the overall size above. Error checking + // starts after the first arbitrary-length field (ident). + + n = snprintf(p, remain, "<%d>", log_facility + sev); // 13 chars max + remain -= n; + p += n; + +// busybox syslogd time format is different from RFC5424 - it looks for +// spaces and colons at specific offsets to know whether the message +// already has a timestamp. +#if 1 + // busybox syslogd; format-sensitive + n = strftime(p, remain, "%h %e %T ", &now_tm); // 16 chars max + remain -= n; + p += n; + // nonstandard - Milliseconds can't be printed right after the seconds, + // since busybox syslogd looks for the space after the seconds. We can + // stick it in like this, and although the output format is goofy, it + // seems like good information. For example, we'd like: + // Sep 9 05:53:47.191 3dr_solo local0.info pixrc: ... + // but the best we can do is: + // Sep 9 05:53:47 3dr_solo local0.info 191 pixrc: ... + int us = now_us % 1000000; + n = snprintf(p, remain, "+%03d ", us / 1000); // 5 chars + remain -= n; + p += n; +#else + // RFC5424 is different (and allows fractions of a second) + n = strftime(p, remain, "%FT%T", &now_tm); + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; + int us = now_us % 1000000; + n = snprintf(p, remain, ".%03dZ - ", us / 1000); + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; +#endif + + n = snprintf(p, remain, "%s", log_ident); // variable length + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; + + if (log_options & LOG_PID) { + n = snprintf(p, remain, "[%d]", getpid()); // 11 chars max + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; + } + + n = snprintf(p, remain, ": "); // 2 chars + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; + + n = vsnprintf(p, remain, fmt, ap); // variable length + remain -= n; + if (remain <= 0) + goto vsyslog2_out; + p += n; + +vsyslog2_out: + + if (log_options & LOG_PERROR) + fprintf(stderr, "%s\n", msg); + + int len = p - msg; + +#ifdef INCLUDE_HISTOGRAM + static uint64_t hist_dump_us = 0; + uint64_t t1_us = clock_gettime_us(CLOCK_MONOTONIC); +#endif + + // nonblocking only seems to work if send is used (not write) + int r = send(log_fd, msg, len, 0); + +#ifdef INCLUDE_HISTOGRAM + unsigned t_us = clock_gettime_us(CLOCK_MONOTONIC) - t1_us; + unsigned t_ms = (t_us + 500) / 1000; + if (t_ms >= hist_bins) + t_ms = hist_bins - 1; + hist[t_ms]++; + if (t1_us >= hist_dump_us) { + if (hist_dump_us != 0) { + printf("syslog block time histogram (msec)\n"); + int b; + for (b = 0; b < hist_bins; b++) + if (hist[b] != 0) + printf("%2d %d\n", b, hist[b]); + } + hist_dump_us = t1_us + hist_dump_interval_us; + memset(hist, 0, sizeof(hist)); + } +#endif + + if (r == len) + return 0; + else + return 1; + +} // vsyslog2 + +// this function exists solely to let syslog/vsyslog log a drop message +static int syslog1(int sev, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + + int r = vsyslog2(sev, fmt, ap); + + va_end(ap); + + return r; + +} // syslog1 + +void vsyslog(int sev, const char *fmt, va_list ap) +{ + + if (log_dropped > 0 && syslog1(LOG_ERR, "dropped %d messages", log_dropped) == 0) + log_dropped = 0; + + if (vsyslog2(sev, fmt, ap) != 0) + log_dropped++; + +} // vsyslog + +void syslog(int sev, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + + vsyslog(sev, fmt, ap); + + va_end(ap); + +} // syslog diff --git a/flightcode/util/syslog_test.c b/flightcode/util/syslog_test.c new file mode 100644 index 0000000..095ae5c --- /dev/null +++ b/flightcode/util/syslog_test.c @@ -0,0 +1,164 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "syslog_test.h" + +int syslog_test(void) +{ + int status = 0; + + int fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (fd < 0) { + perror("syslog_test: socket"); + status = 1; + goto syslog_test_exit_0; + } + + if (fcntl(fd, F_SETFL, O_NONBLOCK) != 0) { + perror("syslog_test: fcntl"); + status = 1; + goto syslog_test_exit_1; + } + + // remove it if already there; ignore error + unlink(SYSLOG_SOCK_NAME); + + struct sockaddr_un sa_un; + memset(&sa_un, 0, sizeof(sa_un)); + sa_un.sun_family = AF_UNIX; + strncpy(sa_un.sun_path, SYSLOG_SOCK_NAME, sizeof(sa_un.sun_path) - 1); + if (bind(fd, (struct sockaddr *)&sa_un, sizeof(sa_un)) != 0) { + perror("syslog_test: bind"); + status = 1; + goto syslog_test_exit_1; + } + + openlog("test", 0, LOG_LOCAL0); + + char msg[256]; + + // should be nothing there + if (recv(fd, msg, sizeof(msg), 0) != -1 || (errno != EAGAIN && errno != EWOULDBLOCK)) { + fprintf(stderr, "syslog_test: unexpected message\n"); + status = 1; + goto syslog_test_exit_2; + } + + syslog(LOG_INFO, "hello!"); + + // should be a message + if (recv(fd, msg, sizeof(msg), 0) <= 0) { + fprintf(stderr, "syslog_test: expected message (%s)\n", strerror(errno)); + status = 1; + goto syslog_test_exit_2; + } + + // should be nothing there + if (recv(fd, msg, sizeof(msg), 0) != -1 || (errno != EAGAIN && errno != EWOULDBLOCK)) { + fprintf(stderr, "syslog_test: unexpected message\n"); + status = 1; + goto syslog_test_exit_2; + } + + // Test dropping messages: Send more than can be queued, then verify we + // get fewer than that back. On the next send, a "drop" message should be + // inserted, so verify that after clearing out the queue, one more message + // causes two to be received. + + // fill it up such that it drops messages + int msgs_sent = 200; + int n; + for (n = 0; n < msgs_sent; n++) + syslog(LOG_INFO, "hello!"); + + // should get fewer than that back + n = 0; + int r; + while ((r = recv(fd, msg, sizeof(msg), 0)) > 0) { + // msg[r] = '\0'; // msg is not '\0' terminated + // printf("%s\n", msg); + n++; + } + if (n >= msgs_sent) { + // this might mean /proc/sys/net/unix/max_dgram_qlen > msgs_sent + fprintf(stderr, "syslog_test: received too many messages\n"); + status = 1; + goto syslog_test_exit_2; + } + + // on the next one, we get the drop message then the new message + syslog(LOG_INFO, "hello!"); + n = 0; + while ((r = recv(fd, msg, sizeof(msg), 0)) > 0) { + // msg[r] = '\0'; // msg is not '\0' terminated + // printf("%s\n", msg); + n++; + } + if (n != 2) { + fprintf(stderr, "syslog_test: did not get drop message\n"); + status = 1; + goto syslog_test_exit_2; + } + + // Verify overflowing the maximum message length + + for (n = 0; n < SYSLOG_MSG_MAX; n++) { + // create a string of length n + memset(msg, 'X', n); + msg[n] = '\0'; + syslog(LOG_INFO, msg); + memset(msg, 0, sizeof(msg)); + if ((r = recv(fd, msg, sizeof(msg), 0)) <= 0) { + fprintf(stderr, "syslog_test: expected message (%s)\n", strerror(errno)); + status = 1; + goto syslog_test_exit_2; + } + msg[r] = '\0'; // msg is not '\0' terminated + // syslog.c was compiled with SYSLOG_MSG_MAX, so the total length of + // the returned message should be less than that + if (strlen(msg) >= (SYSLOG_MSG_MAX - 1)) { + fprintf(stderr, "syslog_test: message overflow\n"); + status = 1; + goto syslog_test_exit_2; + } + // printf("%s\n", msg); + // again, with formatting that would overrun + memset(msg, ' ', n); + msg[n] = '%'; + msg[n + 1] = 'd'; + msg[n + 2] = '\0'; + syslog(LOG_INFO, msg, 1234); + memset(msg, 0, sizeof(msg)); + if ((r = recv(fd, msg, sizeof(msg), 0)) <= 0) { + fprintf(stderr, "syslog_test: expected message (%s)\n", strerror(errno)); + status = 1; + goto syslog_test_exit_2; + } + msg[r] = '\0'; // msg is not '\0' terminated + // syslog.c was compiled with SYSLOG_MSG_MAX, so the total length of + // the returned message should be less than that + if (strlen(msg) >= (SYSLOG_MSG_MAX - 1)) { + fprintf(stderr, "syslog_test: message overflow\n"); + status = 1; + goto syslog_test_exit_2; + } + // printf("%s\n", msg); + } + +syslog_test_exit_2: + closelog(); + unlink(SYSLOG_SOCK_NAME); + +syslog_test_exit_1: + close(fd); + +syslog_test_exit_0: + return status; + +} // syslog_test diff --git a/flightcode/util/syslog_test.h b/flightcode/util/syslog_test.h new file mode 100644 index 0000000..d06cab1 --- /dev/null +++ b/flightcode/util/syslog_test.h @@ -0,0 +1,14 @@ +#ifndef SYSLOG_TEST_H +#define SYSLOG_TEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +extern int syslog_test(void); + +#ifdef __cplusplus +}; +#endif + +#endif /* SYSLOG_TEST_H */ diff --git a/flightcode/util/util.c b/flightcode/util/util.c new file mode 100644 index 0000000..8e5524d --- /dev/null +++ b/flightcode/util/util.c @@ -0,0 +1,131 @@ + +#include +#include + +#if (defined __MACH__ && defined __APPLE__) +#include +#include +#else +#include +#endif + +#include "util.h" + +/* Get current wall-clock time and return it in microseconds since the Unix + * epoch. + * + * CLOCK_REALTIME should be used to get the system's best guess at real time. + * CLOCK_MONOTONIC should be used when jumps in time would cause trouble. + */ +uint64_t clock_gettime_us(clockid_t clock_id) +{ +#if (!defined __MACH__ || !defined __APPLE__) + struct timespec ts; + clock_gettime(clock_id, &ts); + return (uint64_t)ts.tv_sec * 1000000ULL + (ts.tv_nsec + 500) / 1000; +#else + static struct mach_timebase_info tb_info = {0, 0}; + uint64_t now_us = -1; + if (tb_info.denom == 0) + mach_timebase_info(&tb_info); + if (clock_id == CLOCK_MONOTONIC) { + now_us = (mach_absolute_time() * tb_info.numer) / (1000 * tb_info.denom); + } else if (clock_id == CLOCK_REALTIME) { + struct timeval tv; + gettimeofday(&tv, NULL); + now_us = (tv.tv_sec * 1000000ULL) + tv.tv_usec; + } + return now_us; +#endif +} + +/* Set clock. + * This is usually used with CLOCK_REALTIME. + */ +void clock_settime_us(clockid_t clock_id, uint64_t t_us) +{ +#if (!defined __MACH__ || !defined __APPLE__) + struct timespec ts; + ts.tv_sec = t_us / 1000000; + ts.tv_nsec = (t_us % 1000000) * 1000; + clock_settime(clock_id, &ts); +#endif +} + +/* buf must be at least 24 characters */ +const char *clock_tostr_r(uint64_t t_us, char *buf) +{ + time_t s = t_us / 1000000; + unsigned us = t_us % 1000000; + struct tm now_tm; + size_t num_chars; + + localtime_r(&s, &now_tm); + + /* %F is %Y-%m-%d (4+1+2+1+2=10 chars) + * %T is %H:%M:%S (2+1+2+1+2=8 chars) + * total is then 10+1+8+1=20 chars, 21 with EOS */ + num_chars = strftime(buf, 21, "%F %T,", &now_tm); + + /* append 3 chars, rounding to nearest millisecond */ + sprintf(buf + num_chars, "%03d", (us + 500) / 1000); + + return buf; +} + +/* Get a clock and return as human-readable string. */ +const char *clock_gettime_str_r(clockid_t clock_id, char *buf) +{ + return clock_tostr_r(clock_gettime_us(clock_id), buf); +} + +/* convert hex digit to integer */ +int hex_aton(char a) +{ + if (a >= '0' && a <= '9') + return a - '0'; + else if (a >= 'a' && a <= 'f') + return 10 + a - 'a'; + else if (a >= 'A' && a <= 'F') + return 10 + a - 'A'; + else + return -1; +} + +/* convert mac from string ("04:8d:38:7f:1e:20") to array of six bytes */ +uint8_t *mac_aton(const char *mac_string, uint8_t *mac_bytes) +{ + int i; + + for (i = 0; i < 6; i++) { + uint8_t b = 0; + int v = hex_aton(*mac_string); + if (v == -1) + return NULL; + b = v * 16; + mac_string++; + v = hex_aton(*mac_string); + if (v == -1) + return NULL; + b += v; + mac_string++; + if (i < 5 && *mac_string == '\0') + return NULL; + mac_string++; + mac_bytes[i] = b; + } + + return mac_bytes; + +} /* mac_aton */ + +/* convert array of six bytes to mac as string (e.g. "04:8d:38:7f:1e:20") */ +char *mac_ntoa(uint8_t *mac_bytes, char *mac_string) +{ + + sprintf(mac_string, "%02x:%02x:%02x:%02x:%02x:%02x", mac_bytes[0], mac_bytes[1], mac_bytes[2], + mac_bytes[3], mac_bytes[4], mac_bytes[5]); + + return mac_string; + +} /* mac_ntoa */ diff --git a/flightcode/util/util.h b/flightcode/util/util.h new file mode 100644 index 0000000..1d1e5b3 --- /dev/null +++ b/flightcode/util/util.h @@ -0,0 +1,31 @@ +#ifndef UTIL_H +#define UTIL_H + +#include + +#if (defined __MACH__ && defined __APPLE__) +typedef int clockid_t; +#define CLOCK_REALTIME 0 +#define CLOCK_MONOTONIC 1 +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +extern uint64_t clock_gettime_us(clockid_t clock_id); +extern void clock_settime_us(clockid_t clock_id, uint64_t t_us); +extern const char *clock_tostr_r(uint64_t t_us, char *buf); +extern const char *clock_gettime_str_r(clockid_t clock_id, char *buf); +extern int hex_aton(char a); +#define MAC_STRING_LEN 18 +extern char *mac_ntoa(uint8_t *mac_bytes, char *mac_string); +extern uint8_t *mac_aton(const char *mac_string, uint8_t *mac_bytes); + +#ifdef __cplusplus +}; +#endif + +#endif /* UTIL_H */ diff --git a/flightcode/util/util_test.c b/flightcode/util/util_test.c new file mode 100644 index 0000000..b1d3bb5 --- /dev/null +++ b/flightcode/util/util_test.c @@ -0,0 +1,35 @@ +#include +#include +#include +#include "util.h" +#include "util_test.h" + +int util_test(void) +{ + int status = 0; + uint64_t t0, t1; + + // CLOCK_REALTIME incrementing + t0 = clock_gettime_us(CLOCK_REALTIME); + usleep(10000); + t1 = clock_gettime_us(CLOCK_REALTIME); + if (t1 == t0) { + fprintf(stderr, "util_test: CLOCK_REALTIME did not change\n"); + status = 1; + goto util_test_exit_0; + } + + // CLOCK_MONOTONIC incrementing + t0 = clock_gettime_us(CLOCK_MONOTONIC); + usleep(10000); + t1 = clock_gettime_us(CLOCK_MONOTONIC); + if (t1 == t0) { + fprintf(stderr, "util_test: CLOCK_MONOTONIC did not change\n"); + status = 1; + goto util_test_exit_0; + } + +util_test_exit_0: + return status; + +} // util_test diff --git a/flightcode/util/util_test.h b/flightcode/util/util_test.h new file mode 100644 index 0000000..fd7df4c --- /dev/null +++ b/flightcode/util/util_test.h @@ -0,0 +1,14 @@ +#ifndef UTIL_TEST_H +#define UTIL_TEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +extern int util_test(void); + +#ifdef __cplusplus +}; +#endif + +#endif /* UTIL_TEST_H */ diff --git a/flightcode/video/README b/flightcode/video/README new file mode 100644 index 0000000..88c46b1 --- /dev/null +++ b/flightcode/video/README @@ -0,0 +1,54 @@ +The Solo video pipeline exists in this folder. The pipeline works as follows: + + ================= + | | + | SOLO | + | | + | vidout | + ================= + | + | + | + V + ================= + | app_streamer | ========== + | | | | | | + | | ARTOO >-->|----->| mobile | + | | | | | + | -> hdmiout | ========== + ================= + | + | + | + V + ================= + | MONITOR | + ================= + +vidout: +vidout is always running on Solo and handles resolution size changes +automatically. It always sends rtp-encapsulated h.264 720p video +to Artoo on port 5550. Depending on network conditions, it will reduce +the framerate of the video. Specifically, if the number of transmit +retries on Solo eclipses 200, the framerate will be reduced based on the +following: + +30fps -> 24fps -> 15fps -> 10fps -> 5fps + +If the link continues to be bad, the framerate is held at 5fps. Further work +could reduce the GOP or QUANT to reduce bitrate even further. Additional +work should be done to support streaming different resolutions as well. + +app_streamer: +app_streamer on Artoo takes data from port 5550 and passes it to +both the hdmiout as well as any connected mobile devices. It sends full +frames at a time to mobile devices, but passes data byte-by-byte to hdmiout. + +hdmiout: +hdmiout is always running, but waits to detect a connected HDMI cable before +starting the decoding pipeline. It puts 720p video out on the HDMI port of Artoo +using the mfw_isink gstreamer plugin. + +cleanlibs.sh: +cleanlibs is used to remove old Sculpture libraries that are no longer in use. +This is installed as a startup script, but can probably be removed in the future. diff --git a/flightcode/video/app/Makefile b/flightcode/video/app/Makefile new file mode 100644 index 0000000..d007298 --- /dev/null +++ b/flightcode/video/app/Makefile @@ -0,0 +1,45 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../../util ../../ini ../../ini/cpp +INCS = -I../../util -I../../ini -I../../ini/cpp + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) + +LIBS = + +SRCS_CPP = app_streamer.cpp +SRCS_CPP += INIReader.cpp +SRCS_C = util.c +SRCS_C += ini.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = app_streamer + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/video/app/app_streamer.cpp b/flightcode/video/app/app_streamer.cpp new file mode 100644 index 0000000..273e87e --- /dev/null +++ b/flightcode/video/app/app_streamer.cpp @@ -0,0 +1,334 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "INIReader.h" +#include "net_wmm.h" +#include "util.h" + +// This is the module that runs on the controller, relaying video from Solo to +// the currently connected App. + +// App's IP will appear in this file when app connects +static const char *app_ip_filename = "/var/run/solo_app.ip"; + +// Listen for video packets on this UDP port +static const unsigned my_port = 5550; + +// Send video packets to this UDP port (in app) +static const unsigned app_port = 5600; + +// Maximum packet size we will forward +// Video packets all fit in one Enet frame (~1500 bytes) +static const unsigned max_packet_bytes = 2000; + +// Stats +static unsigned packet_count = 0; +static unsigned byte_count = 0; + +// Frame-based transmits +static const unsigned max_frame_packets = 128; +struct pkt { + char buf[max_packet_bytes]; + uint16_t len; +}; + +// Get app IP from file +// Note that returned value is network byte order +static in_addr_t get_app_ip(const char *filename) +{ + + int fd = open(filename, O_RDONLY); + if (fd < 0) + return htonl(INADDR_NONE); + + // max reasonable IP length is 3+1+3+1+3+1+3=15 + char ip_buf[40]; + memset(ip_buf, 0, sizeof(ip_buf)); + ssize_t n_bytes = read(fd, ip_buf, sizeof(ip_buf) - 1); + close(fd); + if (n_bytes <= 0) { + return htonl(INADDR_NONE); + } + + // IP address is typically "10.1.1.10\n"; inet_addr accepts the '\n' + return inet_addr(ip_buf); + +} // get_app_ip + +// Create socket used to send and receive video packets. +// To support receive, bind the socket to the video port, any interface. +// To support send, set the TOS we want for video packets. +static int create_socket(int tos) +{ + int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + syslog(LOG_ERR, "creating socket"); + return -1; + } + + // bind to the input port on any interface + struct sockaddr_in my_addr; + memset(&my_addr, 0, sizeof(my_addr)); + my_addr.sin_addr.s_addr = htonl(INADDR_ANY); + my_addr.sin_port = htons(my_port); + if (bind(fd, (struct sockaddr *)&my_addr, sizeof(my_addr)) < 0) { + syslog(LOG_ERR, "binding socket"); + return -1; + } + + // set TOS for output packets + if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) != 0) { + syslog(LOG_ERR, "setting socket options"); + return -1; + } + + return fd; + +} // create_socket + +// Create the local socket used to send and video packets to the local stream (HDMI output). +// No need to bind as we just dump data across. +static int create_local_socket(void) +{ + int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + syslog(LOG_ERR, "creating local stream socket"); + return -1; + } + + return fd; +} // create_local_socket + +int main(int argc, const char *argv[]) +{ + openlog("streamer", LOG_NDELAY, LOG_LOCAL3); + + syslog(LOG_INFO, "starting: built " __DATE__ " " __TIME__); + + INIReader reader("/etc/sololink.conf"); + + if (reader.ParseError() < 0) { + syslog(LOG_ERR, "can't load /etc/sololink.conf"); + return -1; + } + + int app_check_interval_s = reader.GetInteger("video", "vidAppInt", 1); + unsigned app_check_interval_us = app_check_interval_s * 1000000; + + int log_interval_s = reader.GetInteger("video", "vidLogInt", 10); + unsigned log_interval_us = log_interval_s * 1000000; + + int app_video_tos = reader.GetInteger("video", "vidAppTos", 0xbf); + + int fd = create_socket(app_video_tos); + if (fd < 0) + return 1; // error already logged + + int local_fd = create_local_socket(); + if (local_fd < 0) + return 1; // error already logged + + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + + // most recent log time, init to "now" for calculation purposes + // (first message might be a bit off) + uint64_t log_last_us = now_us; + + // next log time + uint64_t log_time_us = log_last_us + log_interval_us; + + // next time to check for app IP change + uint64_t app_check_us = now_us; + + in_addr_t app_ip = htonl(INADDR_NONE); + struct sockaddr_in app_addr; + + // Setup the local streaming port + struct sockaddr_in local_addr; + memset(&local_addr, 0, sizeof(local_addr)); + local_addr.sin_family = AF_INET; + local_addr.sin_addr.s_addr = inet_addr("127.0.0.1"); + local_addr.sin_port = htons(app_port); // Same port as the app + + // For sending full frame packets + struct pkt frame_pkts[max_frame_packets]; + unsigned frame_pkt_idx = 0; + bool frame_complete = false; + unsigned i; + int small_pkt_count = 0; + + // For sequence monitoring + int seq = 0; + int last_seq = 0; + int drops = 0; + + const int num_fds = 1; + struct pollfd fd_set[num_fds]; + memset(fd_set, 0, sizeof(fd_set)); + fd_set[0].fd = fd; + fd_set[0].events = POLLIN; + + // handle downlink video packets + while (1) { + + // Calculate timeout such that if it expires, we know that it will be + // time to log status or check the app IP (the +1 at the end). E.g if + // log time is 999 usec later than now, delay 1 msec (not zero). + uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC); + int timeout_ms; + // timeout based on log time or app check time, whichever is sooner + if (log_time_us < app_check_us) + timeout_ms = (log_time_us - now_us) / 1000 + 1; + else + timeout_ms = (app_check_us - now_us) / 1000 + 1; + if (timeout_ms < 0) + // handle race condition where we check times at the bottom of + // this loop, then calculate the delay a bit later (here) + timeout_ms = 0; + + // If poll returns with an error, fd_set is unmodified, so let's make + // sure we don't false-detect a ready socket + fd_set[0].revents = 0; + + int pollrc = poll(fd_set, num_fds, timeout_ms); + + if ((pollrc > 0) && (fd_set[0].revents & POLLIN)) { + char pkt_buf[max_packet_bytes]; + + ssize_t pkt_bytes = recv(fd, pkt_buf, sizeof(pkt_buf), 0); + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + + // We shouldn't get any errors from recv. If we do, something new or + // exciting is going on. Log a message, and to avoid soaking the CPU, + // sleep a second before looking for another packet. The sleep will + // disrupt any video that might be playing, but again, we should not + // get any errors here, so the main goal is to let the rest of the + // system run, even at the cost of bad video. + + if (pkt_bytes <= 0) { + syslog(LOG_ERR, "recv returned %d", pkt_bytes); + sleep(1); + } else { + if (sendto(local_fd, pkt_buf, pkt_bytes, 0, (struct sockaddr *)&local_addr, + sizeof(local_addr)) != pkt_bytes) { + // Another error we should never see + syslog(LOG_ERR, "sendto local returned error"); + sleep(1); + } + // Pull the sequence number off the header + seq = pkt_buf[2] << 8 | pkt_buf[3]; + if (seq > (last_seq + 1) && last_seq && seq > last_seq) + drops += (seq - last_seq - 1); + + last_seq = seq; + + // Copy this packet into the frame packet buffer + memcpy(frame_pkts[frame_pkt_idx].buf, pkt_buf, pkt_bytes); + frame_pkts[frame_pkt_idx].len = pkt_bytes; + + // If this is a full frame send it. Or, if its a tiny packet (black screen) send + // it. + // Or if we're out of space, send it all. + if ((pkt_buf[12] == 0x5c && pkt_buf[13] == 0x81) || + frame_pkt_idx == max_frame_packets - 1) + frame_complete = true; + + // We look for small packets. If we get 3 in a row then we start sending them, + // as it could be a black frame. Otherwise, don't send them until we get a + // new frame. 128 bytes consist a "small" packet. Black frames tend to be <100B. + if (pkt_bytes < 128) { + if (small_pkt_count >= 2) + frame_complete = true; + else + ++small_pkt_count; + } else if (small_pkt_count > 0) + small_pkt_count = 0; + + // stats are for downlink packets (vs. forwarded packets) + packet_count++; + byte_count += pkt_bytes; + + // If we have a complete frame or we have no more room, send it along + if (frame_complete) { + if (app_ip != htonl(INADDR_NONE)) { + // Send the entire frame of packets + for (i = 0; i <= frame_pkt_idx; ++i) { + if (sendto(fd, frame_pkts[i].buf, frame_pkts[i].len, 0, + (struct sockaddr *)&app_addr, + sizeof(app_addr)) != frame_pkts[i].len) { + // Another error we should never see, even if the app has + // disconnected. + syslog(LOG_ERR, "sendto returned error"); + sleep(1); + } + } + } + // Reset the frame pkt index, frame_complete flag + frame_pkt_idx = 0; + frame_complete = false; + } else { + // Check if our buffer is full, discard if so + ++frame_pkt_idx; + if (frame_pkt_idx >= max_frame_packets) { + frame_pkt_idx = 0; + continue; + } + } + + } // if (pkt_bytes...) + + } else // if ((pollrc > 0) && (fd_set[0].revents & POLLIN)) + { + // Timeout or error. In either case, we log or check app if it is + // time then just try again. + now_us = clock_gettime_us(CLOCK_MONOTONIC); + } + + // log stats if it is time + if (now_us > log_time_us) { + uint64_t interval_us = now_us - log_last_us; + unsigned packets_sec = (packet_count * 1000000ULL) / interval_us; + unsigned bytes_sec = (byte_count * 1000000ULL) / interval_us; + + syslog(LOG_INFO, "pkts=%i bytes=%i drops=%i", packets_sec, bytes_sec, drops); + + packet_count = 0; + byte_count = 0; + drops = 0; + + log_last_us = now_us; + log_time_us += log_interval_us; + } + + // check for change in app IP if it is time + if (now_us > app_check_us) { + in_addr_t ip = get_app_ip(app_ip_filename); + if (app_ip != ip) { + app_ip = ip; + if (app_ip == htonl(INADDR_NONE)) { + syslog(LOG_INFO, "app disconnected"); + } else { + memset(&app_addr, 0, sizeof(app_addr)); + app_addr.sin_addr.s_addr = app_ip; + app_addr.sin_port = htons(app_port); + struct in_addr app_in_addr = {app_ip}; + syslog(LOG_INFO, "app connected at %s", inet_ntoa(app_in_addr)); + } + } + app_check_us = now_us + app_check_interval_us; + } + + } // while (1) + +} // main diff --git a/flightcode/video/cleanLibs.sh b/flightcode/video/cleanLibs.sh new file mode 100755 index 0000000..7445c05 --- /dev/null +++ b/flightcode/video/cleanLibs.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +RWFS=/mnt/rootfs.rw +LIBDIR=/usr/lib + +if [ -e $RWFS/$LIBDIR/sndast* ] || + [ -e $RWFS/$LIBDIR/libvpu* ] || + [ -e $RWFS/$LIBDIR/libfsl* ] || + [ -e $RWFS/$LIBDIR/gstrea* ] ; then + + #Remove all the sculpture libraries + rm -rf $RWFS/$LIBDIR/sndast + rm -rf $RWFS/$LIBDIR/libvpu* + rm -rf $RWFS/$LIBDIR/libfslvpu* + rm -rf $RWFS/$LIBDIR/gstreamer* + rm -rf $RWFS/$LIBDIR/.*libfslvpu* + + #Remove the gstreamer registry + rm -f ~/.gstreamer-0.10/registry.arm.bin + + #Rebuild the gstreamer library + gst-inspect > /dev/null 2>&1 + +fi diff --git a/flightcode/video/hdmi/Makefile b/flightcode/video/hdmi/Makefile new file mode 100644 index 0000000..15e9114 --- /dev/null +++ b/flightcode/video/hdmi/Makefile @@ -0,0 +1,33 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +PKGCONFIG = `pkg-config gstreamer-0.10 --cflags --libs` + +CFLAGS += $(PKGCONFIG) +CFLAGS += -Wall + +all: out + +out: + $(CC) $(CFLAGS) hdmiout.c -o hdmiout + +clean: + $(RM) *.o *~ hdmiout + +BASE := ../../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/video/hdmi/hdmiout.c b/flightcode/video/hdmi/hdmiout.c new file mode 100644 index 0000000..bc035a7 --- /dev/null +++ b/flightcode/video/hdmi/hdmiout.c @@ -0,0 +1,318 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* For setting the alpha IOCTL */ +#define MXCFB_SET_GBL_ALPHA _IOW('F', 0x21, struct mxcfb_gbl_alpha) + +#define VID_PORT 5600 // The video port from app_streamer +#define OOB_PORT 5551 // The oob data port + +/* Maximum length of an sprop string */ +const int sprop_max_len = 256; + +GstElement *pipeline; +GstElement *source; +GstElement *depayloader; +GstElement *decoder; +GstElement *sink; + +/* Creates a pipeline of the form + * udpsrc ! rtph264depay ! vpudec ! mfw_isink + * and initializes all default parameters + */ +int create_pipeline(int port, char *sprop) +{ + /* Build the pipeline */ + pipeline = gst_pipeline_new("video_stream"); + source = gst_element_factory_make("udpsrc", "vidsrc"); + depayloader = gst_element_factory_make("rtph264depay", "depayloader"); + decoder = gst_element_factory_make("vpudec", "decoder"); + sink = gst_element_factory_make("mfw_isink", "sink"); + + if (!pipeline || !source || !depayloader || !decoder || !sink) { + g_printerr("One element could not be created. Exiting.\n"); + return -1; + } + + /* Caps for the udpsrc. rtph264depay needs these */ + GstCaps *caps = gst_caps_new_simple("application/x-rtp", "media", G_TYPE_STRING, "video", + "clock-rate", G_TYPE_INT, 90000, "encoding-name", + G_TYPE_STRING, "H264", "sprop-parameter-sets", + G_TYPE_STRING, sprop, "payload", G_TYPE_INT, 96, NULL); + + g_object_set(source, "caps", caps, NULL); + gst_caps_unref(caps); + + /* Set the udpsrc port */ + g_object_set(G_OBJECT(source), "port", port, NULL); + + /* For non-choppy video output */ + g_object_set(G_OBJECT(decoder), "low-latency", 1, NULL); + + /* we add all elements into the pipeline */ + gst_bin_add_many(GST_BIN(pipeline), source, depayloader, decoder, sink, NULL); + + /* we link the elements together */ + gst_element_link_many(source, depayloader, decoder, sink, NULL); + + return 0; +} + +/* destroys the pipeline when shutting down */ +int destroy_pipeline(void) +{ + gst_element_set_state(pipeline, GST_STATE_NULL); + g_object_unref(pipeline); + return 0; +} + +/* Restarts the pipeline without destroying it. Accepts + * a new sprop parameter as an arguement. This assumes + * the pipeline needs to be restarted but not destroyed when + * a resolution change occurs to the pipe */ +int restart_pipeline(char *sprop) +{ + gst_element_set_state(pipeline, GST_STATE_NULL); + + /* Caps for the udpsrc. rtph264depay needs these */ + GstCaps *caps = gst_caps_new_simple("application/x-rtp", "media", G_TYPE_STRING, "video", + "clock-rate", G_TYPE_INT, 90000, "encoding-name", + G_TYPE_STRING, "H264", "sprop-parameter-sets", + G_TYPE_STRING, sprop, "payload", G_TYPE_INT, 96, NULL); + + g_object_set(source, "caps", caps, NULL); + gst_caps_unref(caps); + + gst_element_set_state(pipeline, GST_STATE_READY); + gst_element_set_state(pipeline, GST_STATE_PLAYING); + + return 0; +} + +/* Checks the status of the HDMI connection to + * determine if we should start or shut down the + * pipeline + */ +int hdmi_connected(void) +{ + char buf[64]; + memset(buf, 0, 64); + + // Read the cable_state from the sysfs + int fd = + open("/sys/devices/soc0/soc.1/20e0000.hdmi_video/cable_state", O_RDONLY | O_NONBLOCK, 0); + if (fd < 0) { + syslog(LOG_ERR, "Unable to open cable state file"); + // By default assume its connected + goto return_connected; + } + + int len = 0; + len = read(fd, buf, 64); + if (len <= 0) { + syslog(LOG_ERR, "Unable to read cable state file"); + close(fd); + goto return_connected; + } + + // Strip the tailing return + buf[len - 1] = 0; + + if (!strcmp(buf, "plugout")) { + close(fd); + return 0; + } else + close(fd); + +return_connected: + return 1; +} + +/* For handling ctrl-c +*/ +void int_handler(int sig) +{ + destroy_pipeline(); + exit(0); +} + +/* For setting the vsalpha. This eliminates the need for + * a VSALPHA setting in the environment variables, which + * did not always seem to work + */ +int set_alpha(void) +{ + int fd; + struct mxcfb_gbl_alpha { + int enable; + int alpha; + } g_alpha; + + g_alpha.alpha = 0; // alpha value + g_alpha.enable = 1; + + if ((fd = open("/dev/fb0", O_RDWR)) < 0) { + syslog(LOG_ERR, "Unable to open frame buffer 0"); + return -1; + } + + if (ioctl(fd, MXCFB_SET_GBL_ALPHA, &g_alpha) < 0) { + syslog(LOG_ERR, "Set global alpha failed"); + close(fd); + return -1; + } + + close(fd); + return 0; +} + +/* Open the out-of-band socket for sending and + * receiving any out-of-band data, such as a new + * resolution or an error count. + */ +int open_socket(void) +{ + int fd; + struct sockaddr_in addr; + int tos = 0xFF; + + /* create a UDP socket */ + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) + return -1; + + /* Bind the socket to any IP, OOB_PORT */ + memset((char *)&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(OOB_PORT); + + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + return -1; + + /* Set the socket to VI priority */ + if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) < 0) + return -1; + + fcntl(fd, F_SETFL, O_NONBLOCK); // set to non-blocking + + return fd; +} + +/* Looks for data on the OOB port. If available, it + * returns the width, height and framerate of the + * stream. + */ +int oob_receive_sprop(int fd, char *sprop) +{ + struct sockaddr_in addr; + socklen_t addrlen = sizeof(addr); + char buf[sprop_max_len]; + memset(buf, 0, sprop_max_len); + + int recvlen = recvfrom(fd, buf, sprop_max_len, 0, (struct sockaddr *)&addr, &addrlen); + + if (recvlen > 0) { + /* Sanity check that we got a null terminated string */ + if (buf[sprop_max_len - 1] != 0) { + syslog(LOG_INFO, "Got a bad sprop string, ignoring"); + return 0; + } + memset(sprop, 0, sprop_max_len); + strcpy(sprop, buf); + } + + return recvlen; +} + +/* The main entry point. Arguments are currently only + * for gstreamer + */ +int main(int argc, char *argv[]) +{ + char sprop[sprop_max_len]; + char last_sprop[sprop_max_len]; + memset(sprop, 0, sprop_max_len); + memset(last_sprop, 0, sprop_max_len); + int oob_sock_fd; + + /* sig handler to shut down the pipeline */ + signal(SIGINT, int_handler); + + openlog("hdmi", LOG_NDELAY, LOG_LOCAL3); + + /* Set the alphablending off for isink */ + set_alpha(); + + /* Sit here and wait until the HDMI is connected */ + syslog(LOG_INFO, "Waiting for an HDMI connection"); + while (!hdmi_connected()) + sleep(1); + + /* Initialize GStreamer */ + syslog(LOG_INFO, "Initializing HDMI pipeline"); + gst_init(&argc, &argv); + + /* Open the oob socket */ + oob_sock_fd = open_socket(); + if (oob_sock_fd <= 0) { + syslog(LOG_ERR, "Unable to open OOB socket"); + return -1; + } + + /* Wait for some OOB data */ + while (1) { + if (oob_receive_sprop(oob_sock_fd, sprop) > 0) { + syslog(LOG_INFO, "Received sprop"); + break; + } + sleep(1); + } + memcpy(last_sprop, sprop, sprop_max_len); + + /* Create the pipeline */ + syslog(LOG_INFO, "Creating HDMI pipeline"); + if (create_pipeline(VID_PORT, sprop) < 0) + return -1; + + /* Start playing */ + syslog(LOG_INFO, "Starting HDMI playing"); + gst_element_set_state(pipeline, GST_STATE_PLAYING); + + // Check the incoming frame size + while (1) { + // Check the HDMI connection state + if (!hdmi_connected()) { + break; // Exit and let inittab restart us + } + + // Look for new data on the OOB port + if (oob_receive_sprop(oob_sock_fd, sprop) > 0) { + /* If this is a new sprop, restart the pipeline. + * We are guaranteed to have a null terminated string + * from the oob_receive_sprop call */ + if (strcmp(sprop, last_sprop) != 0) { + syslog(LOG_INFO, "Received new sprop: %s", sprop); + + /* Restart the pipeline */ + restart_pipeline(sprop); + + memset(last_sprop, 0, sizeof(last_sprop)); + strcpy(last_sprop, sprop); + } + } + usleep(100000); + } + + /* Free resources */ + destroy_pipeline(); + close(oob_sock_fd); + return 0; +} diff --git a/flightcode/video/vid/80211.cpp b/flightcode/video/vid/80211.cpp new file mode 100644 index 0000000..e45b78f --- /dev/null +++ b/flightcode/video/vid/80211.cpp @@ -0,0 +1,382 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * Rate info, contained in station info + * + * Fields commented out are ones where linux/nl80211.h does not say what the + * type is, and we don't need right now, so they are just left out. + */ +typedef struct rate_info { + uint16_t bitrate; /* total bitrate (u16, 100kbit/s) */ + uint8_t mcs; /* mcs index for 802.11n (u8) */ + /* n40_mhz_width: 40 MHz dualchannel bitrate */ + /* short_gi: 400ns guard interval */ + uint32_t bitrate32; /* total bitrate (u32, 100kbit/s) */ + /* max: highest rate_info number currently defined */ + uint8_t vht_mcs; /* MCS index for VHT (u8) */ + uint8_t vht_nss; /* number of streams in VHT (u8) */ + /* n80_mhz_width: 80 MHz VHT rate */ + /* n80p80_mhz_width: 80+80 MHz VHT rate */ + /* n160_mhz_width: 160 MHz VHT rate */ +} rate_info_t; + +/* + * Information retrieved via station info request message + * + * Fields commented out are ones where linux/nl80211.h does not say what the + * type is, and we don't need right now, so they are just left out. + */ +typedef struct station_info { + uint32_t inactive_time; /* time since last activity (u32, msecs) */ + uint32_t rx_bytes; /* total received bytes (u32, from this station) */ + uint32_t tx_bytes; /* total transmitted bytes (u32, to this station) */ + uint64_t rx_bytes64; /* total received bytes (u64, from this station) */ + uint64_t tx_bytes64; /* total transmitted bytes (u64, to this station) */ + int8_t signal; /* signal strength of last received PPDU (u8, dBm) */ + rate_info_t tx_bitrate; /* current unicast tx rate, nested attribute containing + info as possible, see &enum nl80211_rate_info */ + uint32_t rx_packets; /* total received packet (u32, from this station) */ + uint32_t tx_packets; /* total transmitted packets (u32, to this station) */ + uint32_t tx_retries; /* total retries (u32, to this station) */ + uint32_t tx_failed; /* total failed packets (u32, to this station) */ + int8_t signal_avg; /* signal strength average (u8, dBm) */ + /* llid: the station's mesh LLID */ + /* plid: the station's mesh PLID */ + /* plink_state: peer link state for the station (see %enum nl80211_plink_state) */ + rate_info_t rx_bitrate; /* last unicast data frame rx rate, nested attribute, + like NL80211_STA_INFO_TX_BITRATE. */ + /* bss_param: current station's view of BSS, nested attribute containing info as + possible, see &enum nl80211_sta_bss_param */ + /* connected_time: time since the station is last connected */ + /* sta_flags: Contains a struct nl80211_sta_flag_update. */ + uint32_t beacon_loss; /* count of times beacon loss was detected (u32) */ + int64_t t_offset; /* timing offset with respect to this STA (s64) */ + /* local_pm: local mesh STA link-specific power mode */ + /* peer_pm: peer mesh STA link-specific power mode */ + /* nonpeer_pm: neighbor mesh STA power save mode towards non-peer STA */ +} station_info_t; + +static void get_station_info(station_info_t *station_info); +static int station_handler(struct nl_msg *msg, void *arg); +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg); +static int finish_handler(struct nl_msg *msg, void *arg); +static int ack_handler(struct nl_msg *msg, void *arg); + +int get_retries(void) +{ + station_info_t station_info; + static uint32_t retries_last = 0; + unsigned retries; + + get_station_info(&station_info); + + /* retries is logged as the difference from last time */ + if (retries_last == 0) + retries = 0; + else + retries = station_info.tx_retries - retries_last; + + retries_last = station_info.tx_retries; + + return retries; +} + +/* + * Get station info from interface via netlink + * + * This sends the request to the kernel to send us the station info, + * then waits for the response to come back. The response comes back + * as a callback (station_handler): + * + * Normal operation: + * this function sends request + * this function calls nl_recvmsgs; inside that, station_handler is called + * station_handler fills in station_info + * this function calls nl_recvmsgs; inside that, finish_handler is called + * finish_handler clears 'status' + * this function sees status is cleared and returns + * + * Almost everything returned is parsed out and available. Only a few items + * are logged, and only the signal strength is used in the mavlink message, + * but pulling them all out of the netlink message should make it easier to + * log more (or something different) if we want. + */ +static void get_station_info(station_info_t *station_info) +{ + struct nl_sock *nl_sock = NULL; + int nl80211_id = -1; + unsigned int if_index = 0; + uint32_t if_index_uint32; + struct nl_msg *msg = NULL; + struct nl_cb *cmd_cb = NULL; + struct nl_cb *sock_cb = NULL; + void *hdr = NULL; + int num_bytes = -1; + /* This is static because I'm not 100% sure the callbacks that may set it + cannot get called again under some edge condition. Static is safe, + re-entrancy is irrelevant. */ + static volatile int status = 0; + + memset(station_info, 0, sizeof(*station_info)); + + nl_sock = nl_socket_alloc(); + if (nl_sock == NULL) { + fprintf(stderr, "ERROR allocating netlink socket\n"); + status = 1; + goto cleanup; + } + + if (genl_connect(nl_sock) != 0) { + fprintf(stderr, "ERROR connecting netlink socket\n"); + status = 1; + goto cleanup; + } + + nl80211_id = genl_ctrl_resolve(nl_sock, "nl80211"); + if (nl80211_id < 0) { + fprintf(stderr, "ERROR resolving netlink socket\n"); + status = 1; + goto cleanup; + } + + if_index = if_nametoindex("wlan0"); + if (if_index == 0) { + fprintf(stderr, "ERROR getting interface index\n"); + status = 1; + goto cleanup; + } + + msg = nlmsg_alloc(); + if (msg == NULL) { + fprintf(stderr, "ERROR allocating netlink message\n"); + status = 1; + goto cleanup; + } + + cmd_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (cmd_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink command callback\n"); + status = 1; + goto cleanup; + } + + sock_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (sock_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink socket callback\n"); + status = 1; + goto cleanup; + } + + hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, nl80211_id, 0, NLM_F_DUMP, + NL80211_CMD_GET_STATION, 0); + if (hdr == NULL) { + fprintf(stderr, "ERROR creating netlink message\n"); + status = 1; + goto cleanup; + } + + if_index_uint32 = if_index; + if (nla_put(msg, NL80211_ATTR_IFINDEX, sizeof(uint32_t), &if_index_uint32) != 0) { + fprintf(stderr, "ERROR setting message attribute\n"); + status = 1; + goto cleanup; + } + + if (nl_cb_set(cmd_cb, NL_CB_VALID, NL_CB_CUSTOM, station_handler, station_info) != 0) { + fprintf(stderr, "ERROR setting command callback\n"); + status = 1; + goto cleanup; + } + + nl_socket_set_cb(nl_sock, sock_cb); + + num_bytes = nl_send_auto_complete(nl_sock, msg); + if (num_bytes < 0) { + fprintf(stderr, "ERROR sending netlink message\n"); + status = 1; + goto cleanup; + } + + status = 1; + nl_cb_err(cmd_cb, NL_CB_CUSTOM, error_handler, (void *)&status); + nl_cb_set(cmd_cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, (void *)&status); + nl_cb_set(cmd_cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, (void *)&status); + + /* wait for callback to set station_info and set status=0 */ + while (status == 1) + nl_recvmsgs(nl_sock, cmd_cb); + +cleanup: + + if (sock_cb != NULL) + nl_cb_put(sock_cb); + + if (cmd_cb != NULL) + nl_cb_put(cmd_cb); + + if (msg != NULL) + nlmsg_free(msg); + + if (nl_sock != NULL) + nl_socket_free(nl_sock); + +} /* get_station_info */ + +/* + * Called for each station result message that comes back from the kernel. + * We expect only one before finish_handler is called. + */ +static int station_handler(struct nl_msg *msg, void *arg) +{ + station_info_t *station_info = (station_info_t *)arg; + struct nlattr *tb1[NL80211_ATTR_MAX + 1]; + struct nlmsghdr *nl_hdr; + struct genlmsghdr *genl_hdr; + struct nlattr *genl_attr_data; + int genl_attr_len; + struct nlattr *tb2[NL80211_STA_INFO_MAX + 1]; + static struct nla_policy policy2[NL80211_STA_INFO_MAX + 1] = {{0}}; + struct nlattr *tb3[NL80211_RATE_INFO_MAX + 1]; + static struct nla_policy policy3[NL80211_RATE_INFO_MAX + 1] = {{0}}; + + nl_hdr = nlmsg_hdr(msg); + genl_hdr = (struct genlmsghdr *)nlmsg_data(nl_hdr); + genl_attr_data = genlmsg_attrdata(genl_hdr, 0); + genl_attr_len = genlmsg_attrlen(genl_hdr, 0); + + if (nla_parse(tb1, NL80211_ATTR_MAX, genl_attr_data, genl_attr_len, NULL) != 0) { + fprintf(stderr, "ERROR parsing netlink message attributes\n"); + return NL_SKIP; + } + + if (tb1[NL80211_ATTR_STA_INFO] == NULL) { + printf("no data\n"); + return NL_SKIP; + } + + if (nla_parse_nested(tb2, NL80211_STA_INFO_MAX, tb1[NL80211_ATTR_STA_INFO], policy2) != 0) { + printf("ERROR parsing netlink message nested attributes\n"); + return NL_SKIP; + } + + /* Description of what attributes there are is in linux/nl80211.h */ + + /* For each possible attribute, see if it is present in the info we + * got, and if so, copy it to our station_info_t structure. */ + + if (tb2[NL80211_STA_INFO_INACTIVE_TIME] != NULL) + station_info->inactive_time = nla_get_u32(tb2[NL80211_STA_INFO_INACTIVE_TIME]); + + if (tb2[NL80211_STA_INFO_RX_BYTES] != NULL) + station_info->rx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_RX_BYTES]); + + if (tb2[NL80211_STA_INFO_TX_BYTES] != NULL) + station_info->tx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_TX_BYTES]); + + if (tb2[NL80211_STA_INFO_RX_BYTES64] != NULL) + station_info->rx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_RX_BYTES64]); + + if (tb2[NL80211_STA_INFO_TX_BYTES64] != NULL) + station_info->tx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_TX_BYTES64]); + + /* this appears to be signed dBm, not a u8 */ + if (tb2[NL80211_STA_INFO_SIGNAL] != NULL) + station_info->signal = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL]); + + /* tx_bitrate is a nested structure; the station_info points to a nested + * attribute thing that needs to be decoded */ + if (tb2[NL80211_STA_INFO_TX_BITRATE] != NULL && + nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_TX_BITRATE], policy3) == + 0) { + rate_info_t *rate_info = &station_info->tx_bitrate; + if (tb3[NL80211_RATE_INFO_BITRATE] != NULL) + rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]); + if (tb3[NL80211_RATE_INFO_MCS] != NULL) + rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]); + if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL) + rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]); + if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL) + rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]); + if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL) + rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]); + } + + if (tb2[NL80211_STA_INFO_RX_PACKETS] != NULL) + station_info->rx_packets = nla_get_u32(tb2[NL80211_STA_INFO_RX_PACKETS]); + + if (tb2[NL80211_STA_INFO_TX_PACKETS] != NULL) + station_info->tx_packets = nla_get_u32(tb2[NL80211_STA_INFO_TX_PACKETS]); + + if (tb2[NL80211_STA_INFO_TX_RETRIES] != NULL) + station_info->tx_retries = nla_get_u32(tb2[NL80211_STA_INFO_TX_RETRIES]); + + if (tb2[NL80211_STA_INFO_TX_FAILED] != NULL) + station_info->tx_failed = nla_get_u32(tb2[NL80211_STA_INFO_TX_FAILED]); + + /* this appears to be signed dBm, not a u8 */ + if (tb2[NL80211_STA_INFO_SIGNAL_AVG] != NULL) + station_info->signal_avg = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL_AVG]); + + /* rx_bitrate is nested, like tx_bitrate */ + if (tb2[NL80211_STA_INFO_RX_BITRATE] != NULL && + nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_RX_BITRATE], policy3) == + 0) { + rate_info_t *rate_info = &station_info->rx_bitrate; + if (tb3[NL80211_RATE_INFO_BITRATE] != NULL) + rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]); + if (tb3[NL80211_RATE_INFO_MCS] != NULL) + rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]); + if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL) + rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]); + if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL) + rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]); + if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL) + rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]); + } + + if (tb2[NL80211_STA_INFO_BEACON_LOSS] != NULL) + station_info->beacon_loss = nla_get_u32(tb2[NL80211_STA_INFO_BEACON_LOSS]); + + if (tb2[NL80211_STA_INFO_T_OFFSET] != NULL) + station_info->t_offset = (int64_t)nla_get_u64(tb2[NL80211_STA_INFO_T_OFFSET]); + + return NL_SKIP; + +} /* station_handler */ + +/* Never seen this one called, but the examples have it. */ +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) +{ + *(int *)arg = err->error; + return NL_STOP; +} + +/* + * Called after station info message is sent. The calling of this is how we + * know we're done. + * + * Other commands sent to netlink might get several responses back; this is + * more useful in those cases. + */ +static int finish_handler(struct nl_msg *msg, void *arg) +{ + /* arg is a pointer to 'status' in get_station_info() */ + *(int *)arg = 0; + return NL_SKIP; +} + +/* Never seen this one called, but the examples have it. */ +static int ack_handler(struct nl_msg *msg, void *arg) +{ + *(int *)arg = 0; + return NL_STOP; +} diff --git a/flightcode/video/vid/80211.h b/flightcode/video/vid/80211.h new file mode 100644 index 0000000..38fcb96 --- /dev/null +++ b/flightcode/video/vid/80211.h @@ -0,0 +1,6 @@ +#ifndef __80211_H__ +#define __80211_H__ + +int get_retries(void); + +#endif //__80211_H_ diff --git a/flightcode/video/vid/Makefile b/flightcode/video/vid/Makefile new file mode 100644 index 0000000..cbb49c4 --- /dev/null +++ b/flightcode/video/vid/Makefile @@ -0,0 +1,48 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +PKGCONFIG = `pkg-config gstreamer-0.10 --cflags --libs` + +VPATH = ../../ini ../../ini/cpp + +INCS = -I../../ini -I../../ini/cpp -I$(OECORE_TARGET_SYSROOT)/usr/include/libnl3 + +CFLAGS += -Wall $(INCS) +CXXFLAGS += -Wall $(INCS) $(PKGCONFIG) + +LIBS = -lnl-3 -lnl-genl-3 + +SRCS_CPP = INIReader.cpp +SRCS_CPP += vidlaunch.cpp +SRCS_CPP += 80211.cpp +SRCS_C += ini.c + +OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o) + +MAIN = vidlaunch + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS) + +clean: + $(RM) *.o *~ $(MAIN) *Test + +BASE := ../../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/video/vid/vidlaunch.cpp b/flightcode/video/vid/vidlaunch.cpp new file mode 100644 index 0000000..9ef0c20 --- /dev/null +++ b/flightcode/video/vid/vidlaunch.cpp @@ -0,0 +1,587 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "80211.h" +#include "INIReader.h" + +/* Default defines */ +#define DEFAULT_WIDTH 1280 +#define DEFAULT_HEIGHT 720 +#define DEFAULT_FRAMERATE 24 +#define DEFAULT_BITRATE 4000000 // 4mbps + +#define ARTOO_IP "10.1.1.1" // Artoo's IP address. +#define IB_PORT 5550 // The in-band video port +#define OOB_PORT 5551 // The oob data port +#define TOS_VO 0xFF // Max VO bin for the oob port + +/* The loop time in us for polling retries, etc */ +#define CYCLE_TIME_US 200000 + +/* Convert seconds to number of loop cycles */ +#define SECS_TO_CYCLES(s) (s * 1.e6 / CYCLE_TIME_US) + +/* We can currently control one of two things: framerate or bitrate. + * the vary_framerate switch indicates which one to use. + */ +bool vary_framerate; // Whether or not to vary framerate as the control knob + +/* The min/max bitrate and framerate values (come from sololink.conf) */ +int min_framerate; +int max_framerate; +int min_bitrate; +int max_bitrate; + +/* The step at which the bitrate/framerate should be incremented/decremented + * per second based on the retry count */ +int framerate_step; +int bitrate_step; + +/* Variable stream resolution. This means that the + * video streamed to the Artoo will vary in width/height based + * on the input resolution coming from the HDMI input. + */ +bool var_stream_res; + +/* Whether or not to crop the recorded 480p resolution */ +bool crop_record_res; + +/* Pipeline objects */ +GstElement *pipeline; +GstElement *source; +GstElement *converter; +GstElement *convcapsfilter; +GstElement *enccapsfilter; +GstElement *encoder; +GstElement *payloader; +GstElement *sink; + +/* RTP timestamping */ +uint32_t last_timestamp = 0; + +/*Struct containing the width/height/fr/br of the stream */ +struct vidresfr { + int width; + int height; + int fr; + int br; +}; + +/* The input resolution */ +struct vidresfr input_res = {DEFAULT_WIDTH, DEFAULT_HEIGHT, DEFAULT_FRAMERATE, DEFAULT_BITRATE}; + +/* The streaming resolution */ +struct vidresfr stream_res = {DEFAULT_WIDTH, DEFAULT_HEIGHT, DEFAULT_FRAMERATE, DEFAULT_BITRATE}; + +/* VBR quant values for 720p and 480p resolutions. + * This results in roughly 4mbit maxima for both + * 720p and 480p resolutions, with some extreme bitrates + * in the 5mbit range. + */ +#define QUANT_720 35 +#define QUANT_480 30 // We use this for 576p (PAL) as well. + +/* Looks at the input resolution and determines if the + * stream is NTSC or PAL. Returns 1 for resolutions + * greater than or equal to 720, where both NTSC and PAL + * are the same, other than framerate. + */ +#define IS_NTSC() (input_res.height >= 720 || input_res.height == 480) + +/* Looks at the input resolution structure and determines + * if the input is a recording resolution (480p or 576p). + */ +#define IS_RECORD_RES() (input_res.height < 720) + +/* Record the last timestamp from the payloader */ +inline void record_last_timestamp(void) +{ + g_object_get(G_OBJECT(payloader), "timestamp", &last_timestamp, NULL); + syslog(LOG_INFO, "Last timestamp: %u\n", last_timestamp); +} + +/*Set the timestamp to last_timestamp in payloader */ +inline void set_timestamp(void) +{ + g_object_set(G_OBJECT(payloader), "timestamp-offset", last_timestamp, "config-interval", 1, + "pt", 96, NULL); +} + +/* Creates a pipeline of the form: + * mfw_v4l2src ! mfw_ipucsc ! vpuenc ! rtph264pay ! udpsink + * with default parameters for each + */ +int create_pipeline(void) +{ + /* Build the pipeline */ + pipeline = gst_pipeline_new("video_stream"); + source = gst_element_factory_make("mfw_v4lsrc", "vidsrc"); + convcapsfilter = gst_element_factory_make("capsfilter", NULL); + converter = gst_element_factory_make("mfw_ipucsc", "converter"); + enccapsfilter = gst_element_factory_make("capsfilter", NULL); + encoder = gst_element_factory_make("vpuenc", "encoder"); + payloader = gst_element_factory_make("rtph264pay", "payloader"); + sink = gst_element_factory_make("udpsink", "netsink"); + + // Make sure they all got created OK + if (!pipeline || !source || !convcapsfilter || !converter || !enccapsfilter || !encoder || + !payloader || !sink) { + g_printerr("Element(s) could not be created. Exiting.\n"); + return -1; + } + + /* Set capsfilters for the mfw_ipucsc and vpuenc. + * If we are in 640x480 input mode, assume the video + * is cropped down; the GoPro letterboxes the 480p output + * when recording. Do this by taking 60 pixels from the top/bottom. + * Take 72 pixels from top/bottom if its a PAL input. + * Then, upscale on the output of the ipucsc. + */ + GstCaps *enccaps; + if (IS_RECORD_RES() && crop_record_res) { + enccaps = gst_caps_new_simple("video/x-raw-yuv", "width", G_TYPE_INT, input_res.width, + "height", G_TYPE_INT, input_res.height, "crop-top", + G_TYPE_INT, (IS_NTSC() ? 60 : 72), "crop-bottom", G_TYPE_INT, + (IS_NTSC() ? 60 : 72), NULL); + } else { + enccaps = gst_caps_new_simple("video/x-raw-yuv", "width", G_TYPE_INT, input_res.width, + "height", G_TYPE_INT, input_res.height, NULL); + } + g_object_set(convcapsfilter, "caps", enccaps, NULL); + gst_caps_unref(enccaps); + + if (IS_RECORD_RES() && var_stream_res) { + enccaps = gst_caps_new_simple( + "video/x-raw-yuv", "format", GST_TYPE_FOURCC, GST_MAKE_FOURCC('I', '4', '2', '0'), + "width", G_TYPE_INT, stream_res.width, + /*This next line is ugly but, if the user wants a cropped + * recording resolution then we handle the cropped stream + * if it is NTSC or PAL resolution */ + "height", G_TYPE_INT, + stream_res.height + (crop_record_res ? (IS_NTSC() ? 120 : 144) : 0), NULL); + } else { + enccaps = gst_caps_new_simple( + "video/x-raw-yuv", "format", GST_TYPE_FOURCC, GST_MAKE_FOURCC('I', '4', '2', '0'), + "width", G_TYPE_INT, stream_res.width, "height", G_TYPE_INT, stream_res.height, NULL); + } + g_object_set(enccapsfilter, "caps", enccaps, NULL); + gst_caps_unref(enccaps); + + /* Set encoder parameters */ + if (vary_framerate) { + g_object_set(G_OBJECT(encoder), "codec", 6, "framerate-nu", stream_res.fr, + "force-framerate", 1, "seqheader-method", 3, "cbr", 0, "quant", + (IS_RECORD_RES() ? QUANT_480 : QUANT_720), NULL); + } else { + g_object_set(G_OBJECT(encoder), "codec", 6, "framerate-nu", stream_res.fr, + "force-framerate", 1, "seqheader-method", 3, "bitrate", (int64_t)stream_res.br, + NULL); + } + + /* Set sink paramters */ + g_object_set(G_OBJECT(sink), "host", ARTOO_IP, "port", IB_PORT, "qos-dscp", 32, NULL); + + /* Set packet type 96 + * NOTE: DO NOT CHANGE PACKET TYPE WITHOUT CONSULTING + * MOBILE GROUP, SPECIFICALLY JON W. + * + * Note: config-interval was previously specified, but it appears + * that the seqheader-method=3 from vpuenc automatically sends out lots + * of sps/pps packets. + */ + g_object_set(G_OBJECT(payloader), "timestamp-offset", last_timestamp, "config-interval", 1, + "pt", 96, NULL); + + /* we add all elements into the pipeline */ + gst_bin_add_many(GST_BIN(pipeline), source, convcapsfilter, converter, enccapsfilter, encoder, + payloader, sink, NULL); + + /* we link the elements together */ + gst_element_link_many(source, convcapsfilter, converter, enccapsfilter, encoder, payloader, + sink, NULL); + + return 0; +} + +/* Sets a new framerate for the encoder (vpuenc) and then + * restarts the pipeline. This happens very quickly (a couple + * of frames) + */ +void set_framerate(int fr) +{ + + stream_res.fr = fr; + + syslog(LOG_INFO, "New framerate: %ifps", fr); + + record_last_timestamp(); + + g_object_set(G_OBJECT(encoder), "framerate-nu", stream_res.fr, "force-framerate", 1, NULL); + + // Restart the pipeline + gst_element_set_state(pipeline, GST_STATE_NULL); + gst_element_set_state(pipeline, GST_STATE_READY); + set_timestamp(); + gst_element_set_state(pipeline, GST_STATE_PLAYING); +} + +/* Sets a new bitrate for the encoder (vpuenc) and then + * restarts the pipeline. + */ +void set_bitrate(int br) +{ + + stream_res.br = br; + + syslog(LOG_INFO, "New bitrate: %ibps", br); + + record_last_timestamp(); + + g_object_set(G_OBJECT(encoder), "bitrate", br, NULL); + + // Restart the pipeline + gst_element_set_state(pipeline, GST_STATE_NULL); + gst_element_set_state(pipeline, GST_STATE_READY); + set_timestamp(); + gst_element_set_state(pipeline, GST_STATE_PLAYING); +} + +/* Shuts down and destroys the pipeline */ +int destroy_pipeline(void) +{ + /* Pull off the last timestamp in case the pipeline + * is being restarted. + */ + record_last_timestamp(); + + gst_element_set_state(pipeline, GST_STATE_NULL); + g_object_unref(pipeline); + return 0; +} + +/* For catching ctrl+c */ +void int_handler(int sig) +{ + destroy_pipeline(); + exit(0); +} + +/* Open the out-of-band socket for sending and + * receiving any out-of-band data, such as a new + * resolution or an error count. + */ +int open_oob_socket(void) +{ + int fd; + struct sockaddr_in addr; + int tos = TOS_VO; + + /* create a UDP socket */ + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) + return -1; + + /* Bind the socket to any IP, OOB_PORT */ + memset((char *)&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(OOB_PORT); + + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + return -1; + + /* Set the socket priority */ + if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) < 0) + return -1; + + fcntl(fd, F_SETFL, O_NONBLOCK); // set to non-blocking + + return fd; +} + +/* Send a packet over UDP with the sprop-parameter-sets as + * its payload. + */ +int send_sprop(int fd) +{ + const gchar *gc; + struct sockaddr_in addr; + GstCaps *caps; + GstStructure *str; + GstPad *pad; + int len; + + // Always send to ARTOO's IP on the OOB port. + memset((char *)&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + inet_aton(ARTOO_IP, &addr.sin_addr); + addr.sin_port = htons(OOB_PORT); + + /*Attempt to pull of the sprop-parameter-sets from the + * payloader. If they're OK, send them along */ + if ((pad = gst_element_get_pad(payloader, "src")) != NULL) { + if ((caps = GST_PAD_CAPS(pad)) != NULL) { + if ((str = gst_caps_get_structure(caps, 0)) != NULL) { + gc = gst_structure_get_string(str, "sprop-parameter-sets"); + if (gc == NULL) { + syslog(LOG_ERR, "Unable to get sprop from gstreamer"); + len = 0; + } else + len = strlen(gc); + + if (len > 0) { + if (sendto(fd, (const char *)gc, len, 0, (struct sockaddr *)&addr, + sizeof(addr)) < len) { + syslog(LOG_ERR, "Unable to send data on OOB port\n"); + return -1; + } else + return len; + } + } + } + } + + /* If we sent -1 bytes its entirely possible that + * the pipeline did not have a prop to get yet */ + return -1; +} + +/* The main routine. Argumenets are only for gstreamer, + * currently + */ +int main(int argc, char *argv[]) +{ + + int vid_fd; + int oob_sock_fd; + struct v4l2_frmsizeenum sizes; + uint32_t retries_sec = 0; + int good_retries = 0; + int fr; + int br; + + /* Start the syslog */ + openlog("video", LOG_NDELAY, LOG_LOCAL6); + syslog(LOG_INFO, "main: built " __DATE__ " " __TIME__); + + /* Get the min/max framerate/bitrate values from sololink.conf */ + INIReader reader("/etc/sololink.conf"); + + if (reader.ParseError() < 0) { + syslog(LOG_ERR, "can't load /etc/sololink.conf"); + return -1; + } + + min_framerate = reader.GetInteger("video", "videoMinFR", DEFAULT_FRAMERATE); + max_framerate = reader.GetInteger("video", "videoMaxFR", DEFAULT_FRAMERATE); + min_bitrate = reader.GetInteger("video", "videoMinBR", DEFAULT_BITRATE); + max_bitrate = reader.GetInteger("video", "videoMaxBR", DEFAULT_BITRATE); + framerate_step = reader.GetInteger("video", "videoFRStep", 5); + bitrate_step = reader.GetInteger("video", "videoBRStep", 500000); + var_stream_res = reader.GetBoolean("video", "varStreamRes", true); + crop_record_res = reader.GetBoolean("video", "cropRecordRes", true); + + /* Right now we only support varying framerate or bitrate, not both. + * Throw an error if all values are different */ + if ((min_framerate != max_framerate) && (min_bitrate != max_bitrate)) { + syslog(LOG_ERR, "framerate min/max and bitrate min/max are all different"); + return -1; + } + + /* Check the framerate and bitrate values */ + if (min_framerate < 1 || min_framerate > 60) { + syslog(LOG_ERR, "minimum framerate is out of bounds (1-60)"); + return -1; + } + if (max_framerate < 1 || max_framerate > 60) { + syslog(LOG_ERR, "maximum framerate is out of bounds (1-60)"); + return -1; + } + if (max_framerate < min_framerate) { + syslog(LOG_ERR, "maximum framerate < min framerate"); + return -1; + } + if (min_bitrate < 800000 || min_bitrate > 6000000) { + syslog(LOG_ERR, "minimum bitrate is out of bounds (800000-6000000)"); + return -1; + } + if (max_bitrate < 800000 || max_bitrate > 6000000) { + syslog(LOG_ERR, "maximum bitrate is out of bounds (800000-6000000)"); + return -1; + } + if (max_bitrate < min_bitrate) { + syslog(LOG_ERR, "maximum bitrate < min bitrate"); + return -1; + } + if (framerate_step < 0 || framerate_step > 30) { + syslog(LOG_ERR, "framerate step is out of bounds (1-30)"); + return -1; + } + if (bitrate_step < 100000 || bitrate_step > 2000000) { + syslog(LOG_ERR, "bitrate step is out of bounds (100000 - 2000000)"); + return -1; + } + + /* Determine which mode we're in (fixed framerate or fixed bitrate) */ + if (max_framerate == min_framerate) { + vary_framerate = + false; // Indicate that we should use CBR but change it based on link conditions + syslog(LOG_INFO, "Operating in variable bitrate mode (%i-%i)@%ifps", min_bitrate, + max_bitrate, max_framerate); + } else if (max_bitrate == min_bitrate) { + vary_framerate = true; // Indicate that we should use VBR but vary framerate + syslog(LOG_INFO, "Operating in variable framerate mode (%i-%i)@%ibps", min_framerate, + max_framerate, max_bitrate); + } + + /* Mark if we're using a variable stream resolution */ + if (var_stream_res) + syslog(LOG_INFO, "Using variable streaming resolution"); + + /* Only support cropping in variable frame resolution */ + if (!var_stream_res && crop_record_res) { + syslog(LOG_INFO, + "WARN: Cropped resolutions only supported in variable streaming resolutions"); + crop_record_res = false; + } + + /* Set the bitrate/framerates to the set maxima */ + stream_res.fr = max_framerate; + stream_res.br = max_bitrate; + fr = max_framerate; + br = max_bitrate; + + /* sigint handler to destroy the pipeline */ + signal(SIGINT, int_handler); + + /* Modprobe the driver */ + system("modprobe mxc_v4l2_capture"); + + /* Initialize GStreamer */ + syslog(LOG_INFO, "Initializing"); + gst_init(&argc, &argv); + + /* Open the oob socket */ + oob_sock_fd = open_oob_socket(); + if (oob_sock_fd <= 0) { + syslog(LOG_ERR, "Unable to open OOB socket"); + return -1; + } + + /* Create the pipeline */ + syslog(LOG_INFO, "Creating pipeline"); + if (create_pipeline() < 0) + return -1; + + /* Start playing */ + syslog(LOG_INFO, "Starting play"); + gst_element_set_state(pipeline, GST_STATE_PLAYING); + + /* Open the video fd for the enum_framesizes ioctl call */ + vid_fd = open("/dev/video0", O_RDWR | O_NONBLOCK, 0); + if (vid_fd < 0) { + syslog(LOG_ERR, "Unable to open video device for ioctl"); + destroy_pipeline(); + return -1; + } + + /* The main loop */ + while (1) { + + /* Check the incoming frame size */ + sizes.index = 0; + /* Call the ioctl to get the input resolution */ + if (ioctl(vid_fd, VIDIOC_ENUM_FRAMESIZES, &sizes) < 0) + syslog(LOG_ERR, "Unable to call VIDIOC_ENUM_FRAMESIZES ioctl"); + else { + if (sizes.discrete.width != (unsigned)input_res.width || + sizes.discrete.height != (unsigned)input_res.height) { + syslog(LOG_INFO, "new size: %ix%i", sizes.discrete.width, sizes.discrete.height); + + /* Set the input size */ + input_res.width = sizes.discrete.width; + input_res.height = sizes.discrete.height; + + if (var_stream_res) { + /* Set the streaming resolution to the incoming size for now */ + stream_res.width = input_res.width; + stream_res.height = input_res.height; + } + + /* Destroy the pipeline */ + syslog(LOG_INFO, "Restarting pipeline"); + destroy_pipeline(); + + /* Re-create the pipeline */ + create_pipeline(); + + /* Start playing */ + gst_element_set_state(pipeline, GST_STATE_PLAYING); + } + } + + /* Notify the Artoo of the latest resolution. We do this once per cycle */ + send_sprop(oob_sock_fd); + + /* The framerate/bitrate control is pretty simple. Any time we see + * a retry/sec > 200 we drop the framerate/bitrate by a step. If its + * already at the minimum, we leave it there. If the retries + * are < 200 for 5s we increase the framerate/bitrate by one step up + * to the max. + */ + retries_sec = get_retries() * 1e6 / CYCLE_TIME_US; + if (retries_sec > 200) { + /* Reduce the framerate/bitrate by a step if its not already at the + * minimum value */ + if (vary_framerate && stream_res.fr > min_framerate) { + fr = stream_res.fr - framerate_step; + if (fr < min_framerate) + fr = min_framerate; + } else if (!vary_framerate && stream_res.br > min_bitrate) { + br = stream_res.br - bitrate_step; + if (br < min_bitrate) + br = min_bitrate; + } + good_retries = 0; + } + /* If the good_retries is not at the max, increment it */ + else if (good_retries < SECS_TO_CYCLES(3)) { + if (++good_retries >= SECS_TO_CYCLES(3)) { + /* Increase the framerate/bitrate by a step if its not already at the + * maximum value */ + if (vary_framerate && stream_res.fr < max_framerate) { + fr = stream_res.fr + 2 * framerate_step; + if (fr > max_framerate) + fr = max_framerate; + } else if (!vary_framerate && stream_res.br < max_bitrate) { + br = stream_res.br + 2 * bitrate_step; + if (br > max_bitrate) + br = max_bitrate; + } + /* Reset the good retries so we keep counting up */ + good_retries = 0; + } + } + + /* Finally, set the actual framerate/bitrate */ + if (vary_framerate && stream_res.fr != fr) + set_framerate(fr); + if (!vary_framerate && stream_res.br != br) + set_bitrate(br); + + usleep(CYCLE_TIME_US); + } + + /* Should never get here. */ + + /* Free resources */ + destroy_pipeline(); + return 0; +} diff --git a/flightcode/wdog/Makefile b/flightcode/wdog/Makefile new file mode 100644 index 0000000..f70e6b0 --- /dev/null +++ b/flightcode/wdog/Makefile @@ -0,0 +1,36 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +CFLAGS += -Wall $(INCS) + +SRCS_C = wdog.c + +OBJS = $(SRCS_C:.c=.o) + +MAIN = wdog + +all: $(MAIN) + +$(MAIN): $(OBJS) + $(LINK.c) -o $(MAIN) $(OBJS) + +clean: + $(RM) *.o *~ $(MAIN) + +BASE := ../.. + +fmt: + @python $(BASE)/tools/build/clang-format-run.py --apply + +fmt-diff: + @python $(BASE)/tools/build/clang-format-run.py + +.PHONY: all clean fmt fmt-diff diff --git a/flightcode/wdog/wdog.c b/flightcode/wdog/wdog.c new file mode 100644 index 0000000..1419c1a --- /dev/null +++ b/flightcode/wdog/wdog.c @@ -0,0 +1,106 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* start, kick, or cancel the watchdog */ + +#define DEV_NAME "/dev/watchdog" + +static int usage(void) +{ + printf("usage: wdog -t set new timeout\n"); + printf(" wdog -k kick watchdog\n"); + printf(" wdog -c cancel watchdog\n"); + return 1; +} + +static int set_timeout(int t) +{ + int status = 0; + int fd = open(DEV_NAME, O_RDWR); + if (fd < 0) { + fprintf(stderr, "wdog: ERROR opening %s\n", DEV_NAME); + status = 1; + } else { + if (ioctl(fd, WDIOC_SETTIMEOUT, &t) != 0) { + fprintf(stderr, "wdog: ERROR setting watchdog\n"); + status = 1; + } + close(fd); + } + return status; +} + +static int cancel(void) +{ + int status = 0; + int fd = open(DEV_NAME, O_RDWR); + if (fd < 0) { + fprintf(stderr, "wdog: ERROR opening %s\n", DEV_NAME); + status = 1; + } else { + if (write(fd, "V", 1) != 1) { + fprintf(stderr, "wdog: ERROR canceling watchdog\n"); + status = 1; + } + close(fd); + } + return status; +} + +static int kick(void) +{ + int status = 0; + int fd = open(DEV_NAME, O_RDWR); + if (fd < 0) { + fprintf(stderr, "wdog: ERROR opening %s\n", DEV_NAME); + status = 1; + } else { + if (ioctl(fd, WDIOC_KEEPALIVE, 0) != 0) { + fprintf(stderr, "wdog: ERROR kicking watchdog\n"); + status = 1; + } + close(fd); + } + return status; +} + +int main(int argc, char *argv[]) +{ + int status; + + if (argc == 2) { + if (strcmp(argv[1], "-c") == 0) { + status = cancel(); + } else if (strcmp(argv[1], "-k") == 0) { + status = kick(); + } else { + status = usage(); + } + } else if (argc == 3) { + if (strcmp(argv[1], "-t") == 0) { + char *e; + long int t = strtol(argv[2], &e, 10); + if (e != argv[2] && *e == '\0') { + status = set_timeout(t); + } else { + status = usage(); + } + } else { + status = usage(); + } + } else { + status = usage(); + } + + exit(status); + +} /* main */ diff --git a/gimbal/firmware_helper.py b/gimbal/firmware_helper.py new file mode 100644 index 0000000..e82e209 --- /dev/null +++ b/gimbal/firmware_helper.py @@ -0,0 +1,60 @@ +''' + Helper functions to work with firmware files +''' +import base64 +import zlib +import json + + +def bytearray_to_wordarray(data): + '''Converts an 8-bit byte array into a 16-bit word array''' + wordarray = list() + + for i in range(len(data) / 2): + # Calculate 16 bit word from two bytes + msb = data[(i * 2) + 0] + lsb = data[(i * 2) + 1] + word = (msb << 8) | lsb + wordarray.append(word) + + return wordarray + +def add_checksum(checksum, word): + '''Simple XOR checksum''' + checksum ^= word + return checksum + +def append_checksum(binary): + '''Calculate and append the XOR checksum to the bytearray''' + checksum = 0xFFFF + wordarray = bytearray_to_wordarray(binary) + + # Compute the checksum + for i in range(len(wordarray)): + checksum ^= wordarray[i] + + # Add the checksum to the end of the wordarray + wordarray.extend([checksum & 0xFFFF, (checksum & 0xFFFF) >> 16, 0x0000]) + + # Convert the wordarray back into a bytearray + barray = list() + for i in range(len(wordarray)): + lsb = wordarray[i] & 0xFF + msb = (wordarray[i] >> 8) & 0xFF + barray.append(lsb) + barray.append(msb) + + return barray, checksum + +def load_firmware(filename): + '''Load the image from the JSON firmware file into a byte array''' + with open(filename, "r") as f: + desc = json.load(f) + desc['binary'] = bytearray(zlib.decompress(base64.b64decode(desc['image']))) + return desc + + +def load_binary(filename): + '''Load binary image file into a byte array''' + with open(filename, "rb") as f: + return bytearray(f.read()) diff --git a/gimbal/firmware_loader.py b/gimbal/firmware_loader.py new file mode 100644 index 0000000..597b225 --- /dev/null +++ b/gimbal/firmware_loader.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python + +""" +Utility for loading firmware into the 3DR Gimbal. + +""" + +import sys + +from firmware_helper import append_checksum, load_firmware +import setup_mavlink +import setup_validate + +bootloaderVersionHandler = None +progressHandler = None + +MAVLINK_ENCAPSULATED_DATA_LENGTH = 253 + +DATA_TRANSMISSION_HANDSHAKE_EXITING_MAGIC_WIDTH = 0xFFFF + +class Results: + Success, NoResponse, Timeout, InBoot, Restarting = 'Success', 'NoResponse', 'Timeout', 'InBoot', 'Restarting' + +def decode_bootloader_version(msg): + """The first message handshake contains the bootloader version int the height field as a 16bit int""" + version_major = (msg.height >> 8) & 0xff + version_minor = msg.height & 0xff + return [version_major, version_minor] + +def start_bootloader(link): + """Check if target is in booloader, if not reset into bootloader mode""" + msg = setup_mavlink.get_any_gimbal_message(link) + if msg and setup_mavlink.is_bootloader_message(msg): + return Results.InBoot + + # Signal the target to reset into bootloader mode + setup_mavlink.reset_into_bootloader(link) + + # Wait for the bootloader to send a handshake + timeout_counter = 0 + while timeout_counter < 10: + msg = setup_mavlink.wait_handshake(link) + if msg is None: + setup_mavlink.reset_into_bootloader(link) + timeout_counter += 1 + else: + return Results.Restarting + return Results.NoResponse + +def send_block(link, binary, msg): + sequence_number = msg.width + payload_length = msg.payload + + # Calculate the window of data to send + start_idx = sequence_number * payload_length + end_idx = (sequence_number + 1) * payload_length + + # Clamp the end index from overflowing + if (end_idx > len(binary)): + end_idx = len(binary) + + # Slice the binary image + data = binary[start_idx:end_idx] + + # Pad the data to fit the mavlink message + if len(data) < MAVLINK_ENCAPSULATED_DATA_LENGTH: + data.extend([0] * (MAVLINK_ENCAPSULATED_DATA_LENGTH - len(data))) + + # Send the data with the corrosponding sequence number + setup_mavlink.send_bootloader_data(link, sequence_number, data) + return end_idx + +def upload_data(link, binary): + global progressHandler, bootloaderVersionHandler + + msg = setup_mavlink.wait_handshake(link) + if msg == None: + return Results.NoResponse + + # Emit the bootloader version + if bootloaderVersionHandler: + blver = decode_bootloader_version(msg) + bootloaderVersionHandler(blver[0], blver[1]) + + # Loop until we are finished + end_idx = 0 + retries = 0 + # Note: MAX_RETRIES needs to be longer than the maximum possible flash + # erase time (2 seconds * 6 Sectors = 12 Seconds) + MAX_RETRIES = 15 # Seconds + while end_idx < len(binary): + msg = setup_mavlink.wait_handshake(link) + if msg is None: + if retries > MAX_RETRIES: + return Results.NoResponse + else: + retries += 1 + continue + retries = 0 + + end_idx = send_block(link, binary, msg) + + uploaded_kb = round(end_idx / 1024.0, 2) + total_kb = round(len(binary) / 1024.0, 2) + percentage = int((100.0 * end_idx) / len(binary)) + if progressHandler: + progressHandler(uploaded_kb, total_kb, percentage) + + return Results.Success + +def finish_upload(link): + """Send an "end of transmission" signal to the target, to cause a target reset""" + while True: + setup_mavlink.exit_bootloader(link) + msg = setup_mavlink.wait_handshake(link) + if msg == None: + return Results.Timeout + if msg.width == DATA_TRANSMISSION_HANDSHAKE_EXITING_MAGIC_WIDTH: + break + + if setup_mavlink.wait_for_gimbal_message(link, timeout=10): + return Results.Success + else: + return Results.Timeout + +def load_binary(binary, link, bootloaderVersionCallback=None, progressCallback=None): + global bootloaderVersionHandler, progressHandler + + if progressCallback: + progressHandler = progressCallback + + if bootloaderVersionCallback: + bootloaderVersionHandler = bootloaderVersionCallback + + result = upload_data(link, binary) + if result != Results.Success: + return result + + return finish_upload(link) + \ No newline at end of file diff --git a/gimbal/loadGimbal.py b/gimbal/loadGimbal.py new file mode 100644 index 0000000..be58795 --- /dev/null +++ b/gimbal/loadGimbal.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python + +import subprocess, sys, os, time, glob, argparse, clock +from datetime import datetime +from re import search +from json import loads +SELECT_GPIO = "21" +ENABLE_GPIO = "19" + +firmware_ext = "ax" +version_file = "/AXON_VERSION" + +def setGPIODir(gpio, direction): + dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w") + dir_fd.write(direction) + dir_fd.close() + +def openGPIO(gpio): + # Check and see if the GPIO is already exported + if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)): + # Otherwise export it + exp_fd = open("/sys/class/gpio/export", "w") + exp_fd.write(gpio) + exp_fd.close() + + setGPIODir(gpio, "out"); + +def closeGPIO(gpio): + unexp_fd = open("/sys/class/gpio/unexport", "w") + unexp_fd.write(gpio) + unexp_fd.close() + +def setGPIO(gpio, value): + val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w") + val_fd.write(value) + val_fd.close() + +def openSetClose(gpio, value): + openGPIO(gpio) + setGPIO(gpio, value) + closeGPIO(gpio) + +def writeVerFile(AxonVersion, AxonRelease): + verFile = open(version_file,"w") + verFile.write(AxonVersion + '\n') + verFile.write(AxonRelease + '\n') + verFile.close() + +def removeVerFile(): + try: + os.remove(version_file) + except Exception: + pass + +def disconnectAndExit(): + openSetClose(ENABLE_GPIO, "1") # disable + openSetClose(SELECT_GPIO, "0") # uninit + os.system("echo none > /sys/class/leds/user2/trigger") + sys.exit() + +# return a matching file, or None +def glob_file(pattern): + files = glob.glob(pattern) + if len(files) == 0: + return None + elif len(files) > 1: + print("more than one matching filename (%s)" % str(files)) + return files[0] + + +dev_pattern_usb = "/dev/serial/by-id/usb-3D_Robotics*" + +# return device name or None +def create_usb_serial(timeout=2): + openSetClose(SELECT_GPIO, "1") # init + openSetClose(ENABLE_GPIO, "0") # enable + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + now_us = start_us + timeout_us = timeout * 1000000 + while glob_file(dev_pattern_usb) is None and (now_us - start_us) < timeout_us: + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + time.sleep(0.001) + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + dev_name = glob_file(dev_pattern_usb) + return dev_name + +# Bootloading process +print("Gimbal update startup script v1.1.8") + +parser = argparse.ArgumentParser() +parser.add_argument("file_specified", nargs='?') +args = parser.parse_args() + +if args.file_specified and not os.path.isfile(args.file_specified): + print("File \"%s\" not found" % (args.file_specified)) + sys.exit() + +if args.file_specified: + latest = args.file_specified +else: + # See what versions we have, sorted by file timestamp + files = sorted(glob.glob('/firmware/*.%s' % firmware_ext), key=os.path.getmtime) + if not files: + print("No Gimbal firmware available for update.") + sys.exit() + + # Pop the first (newest) firmware from the array + latest = files[-1] + + # Cleanup old firmwares + if len(files) > 1: + for i in range(len(files) - 1): + print("Removing old firmware: %s" % files[i]) + os.remove(files[i]) + +# Get the versions from px4 json +latest_json = loads(open(latest).read()) +AxonVersion = latest_json['version'] +AxonRelease = latest_json['release'] + +# Set the LED to a blink pattern so the user knows we're updating +os.system("echo timer > /sys/class/leds/user2/trigger") +os.system("echo 1000 > /sys/class/leds/user2/delay_on") +os.system("echo 100 > /sys/class/leds/user2/delay_off") + +pixhawk_usb = create_usb_serial() +if pixhawk_usb is None: + print("No pixhawk found on USB. Exiting.") + disconnectAndExit() +else: + print("Pixhawk found on USB") + +semver = "unknown" +try: + output = subprocess.check_output(["/usr/bin/gimbal_setup", "--port=%s" % pixhawk_usb]) + print(output.strip()) + version = search(ur"v(\d+.\d+.\d+)", output) + if version: + semver = version.group(1) +except Exception: + print("Failed to find an attached gimbal, exiting") + removeVerFile() + disconnectAndExit() + +print("Current gimbal software version %s" % semver) + +# Quit early if the gimbal version is the latest (unless called directly with a firmware) +if semver == AxonVersion and not args.file_specified: + print("Gimbal firmware is current, not updating.") + writeVerFile(AxonVersion, AxonRelease) + disconnectAndExit() + +print("Loading file %s" % (os.path.abspath(latest))) + +# Bootload the gimbal now +ret = subprocess.call(["/usr/bin/gimbal_setup", "--port=%s" % pixhawk_usb, latest]) + +if ret != 0: + print("Unable to update the gimbal, closing") +else: + print("Succesfully updated the gimbal") + writeVerFile(AxonVersion, AxonRelease) + +disconnectAndExit() + diff --git a/gimbal/setup.py b/gimbal/setup.py new file mode 100755 index 0000000..ad069d0 --- /dev/null +++ b/gimbal/setup.py @@ -0,0 +1,478 @@ +#!/usr/bin/env python + +''' +Command-line utility to handle comms to gimbal +''' +import time, os, sys, argparse, time, json +import setup_mavlink, setup_factory_pub + +from firmware_helper import load_firmware, append_checksum +from firmware_loader import load_binary, start_bootloader +from firmware_loader import Results as loader_results +import setup_validate, setup_param, setup_comutation +from setup_comutation import Results as calibration_results + +# Optional imports +try: + import setup_run + import setup_home + import setup_factory_private + import firmware_git_tools +except ImportError: + setup_run = None + setup_home = None + setup_factory_private = None + firmware_git_tools = None + +def handle_file(args, link): + fileExtension = str(args.file).split(".")[-1].lower() + if fileExtension == 'param': + setup_param.load_param_file(args.file, link) + elif fileExtension == 'json': + with open(args.file) as f: + params = json.load(f) + setup_param.restore_params(link, params) + print("Gimbal parameters restored.") + elif fileExtension == 'ax': + # Prepare the binary to load from the compressed .ax file + print('Application firmware_file: %s' % args.file) + firmware = load_firmware(args.file) + binary, checksum = append_checksum(firmware['binary']) + print('Checksum: 0x%04X' % checksum) + + # Start the bootloader + result = start_bootloader(link) + if result == loader_results.NoResponse: + print("No response from gimbal, exiting.") + sys.exit(1) + elif result == loader_results.InBoot: + print('Target already in bootloader mode') + elif result == loader_results.Restarting: + print("Restarted target in bootloader mode") + + def loaderProgressCallback(uploaded_kb, total_kb, percentage): + if percentage == 0: + sys.stdout.write("\rErasing flash...") + else: + sys.stdout.write("\rUploading %2.2fkB of %2.2fkB - %d%%" % (uploaded_kb, total_kb, percentage)) + # The flush is required to refresh the screen on Ubuntu + sys.stdout.flush() + + def bootloaderVersionCallback(major, minor): + print('Bootloader Ver %i.%i' % (major, minor)) + + # Load the binary using the bootloader + result = load_binary(binary, link, bootloaderVersionCallback=bootloaderVersionCallback, progressCallback=loaderProgressCallback) + sys.stdout.write("\n") + sys.stdout.flush(); + if result == loader_results.Success: + print("Upload successful") + + # If the script is being run on a Solo, make sure custom gains are diabled + if setup_factory_private is None: + setup_mavlink.wait_for_gimbal_message(link) + setup_param.set_use_custom_gains(link, 0) + elif result == loader_results.NoResponse: + print("No response from gimbal, exiting.") + sys.exit(1) + elif result == loader_results.Timeout: + print("Timeout") + sys.exit(1) + else: + print("Unknown error while finishing bootloading") + sys.exit(1) + else: + print("File type not supported") + sys.exit(1) + return + +def calibrationProgressCallback(axis, progress, status): + text = "\rCalibrating %s - progress %d%% - %s " % (axis, progress, status) + sys.stdout.write(text) + # The flush is required to refresh the screen on Ubuntu + sys.stdout.flush() + +def eraseCalibration(link): + print('Clearing old calibration values...') + setup_comutation.resetCalibration(link) + print('Calibration values cleared') + +def runCalibration(link): + result = setup_comutation.calibrate(link, calibrationProgressCallback) + sys.stdout.write("\n") + sys.stdout.flush() + + if result == calibration_results.ParamFetchFailed: + print("Failed to get calibration parameters from the gimbal") + return + elif result == calibration_results.CommsFailed: + print("Gimbal failed to communicate calibration progress") + return + + # These results require a reset of the Gimbal + if result == calibration_results.Success: + print("Calibration successful!") + return + elif result == calibration_results.CalibrationExists: + print("A calibration already exists, erase current calibration first (-e)") + return + elif result == calibration_results.PitchFailed: + print("Pitch calibration failed") + return + elif result == calibration_results.RollFailed: + print("Roll calibration failed") + return + elif result == calibration_results.YawFailed: + print("Yaw calibration failed") + return + + # Reset the gimbal + print("Rebooting Gimbal") + setup_mavlink.reset_gimbal(link) + + # Get the calibration data + if result == calibration_results.Success: + calibration = setup_comutation.getAxisCalibrationValues(link) + if calibration: + print("") + for axis in ['pitch', 'roll', 'yaw']: + print("%s: slope=%f intercept=%f" % (axis, calibration[axis]['slope'], calibration[axis]['intercept'])) + else: + print("Error getting calibration values from the gimbal") + return + +def printValidation(link): + valid = setup_validate.validate_version(link) + if valid == setup_validate.Results.Pass: + print("Version \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Version \t- FAIL - please update to latest gimbal software") + else: + print("Version \t- ERROR") + + valid = setup_validate.validate_serial_number(link) + if valid == setup_validate.Results.Pass: + print("Serial Number \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Serial Number \t- FAIL - Serial number was not set (--serialnumber SERIALNUMBER)") + else: + print("Serial Number \t- ERROR") + + valid = setup_validate.validate_date(link) + if valid == setup_validate.Results.Pass: + print("Assembly date \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Assembly date \t- FAIL - assembly date was not set on the factory (--date)") + else: + print("Assembly date \t- ERROR") + + valid = setup_validate.validate_comutation(link) + if valid == setup_validate.Results.Pass: + print("Comutation \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Comutation \t- FAIL - please redo the comutation calibration (-c)") + else: + print("Comutation \t- ERROR") + + valid = setup_validate.validate_joints(link) + if valid == setup_validate.Results.Pass: + print("Joints \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Joints \t- FAIL - redo joint calibration (-j)") + else: + print("Joints \t- ERROR") + + valid = setup_validate.validate_gyros(link) + if valid == setup_validate.Results.Pass: + print("Gyros \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Gyros \t- FAIL - redo gyro calibration (-g)") + else: + print("Gyros \t- ERROR") + + valid = setup_validate.validate_accelerometers(link) + if valid == setup_validate.Results.Pass: + print("Accelerometer\t- PASS") + elif valid == setup_validate.Results.Fail: + print("Accelerometer\t- FAIL - redo accelerometer calibration (-a)") + else: + print("Accelerometer\t- ERROR") + + valid = setup_validate.validate_gains(link) + if valid == setup_validate.Results.Pass: + print("Gains \t- PASS") + elif valid == setup_validate.Results.Fail: + print("Gains \t- WARNING - custom gains are enabled (disable with --customgains=0)") + else: + print("Gains \t- ERROR") + +# Main method when called directly +def command_interface(): + parser = argparse.ArgumentParser() + parser.add_argument("file", nargs='?', help="parameter or firmware file to be loaded into the gimbal", default=None) + parser.add_argument("-p", "--port", help="Serial port or device used for MAVLink bootloading", default=None) + parser.add_argument("--show", help="Show all gimbal parameters", action='store_true') + parser.add_argument("--save", help="Save gimbal parameters to a file", action='store_true') + parser.add_argument("-v", "--validate", help="Check gimbal parameters to see if they have valid values", action='store_true') + parser.add_argument("-d", "--defaults", help="Reset gimbal parameters to default values", action='store_true') + parser.add_argument("-r","--reboot", help="Reboot the gimbal", action='store_true') + parser.add_argument("--eraseapp", help="Erase the application", action='store_true') + parser.add_argument("-c", "--calibrate", help="Run the comutation setup", action='store_true') + parser.add_argument("-f", "--forcecal", help="Force the comutation setup", action='store_true') + parser.add_argument("-e", "--erase", help="Erase calibration values", action='store_true') + parser.add_argument("--customgains", help="Enable the use of custom gains (0 to disable, 1 to enable)", type=int) + parser.add_argument("--charging", help="Set camera charging (0 to disable, 1 to enable)", type=int) + + # Optional commands (not included with sololink tools) + if setup_run: + parser.add_argument("--run", help="run a quick test of the gimbal", action='store_true') + parser.add_argument("--align", help="move the gimbal to the home position", action='store_true') + parser.add_argument("--stop", help="Hold the gimbal at the current position", action='store_true') + parser.add_argument("--wobble", help="Wobble fixture test", action='store_true') + parser.add_argument("--timeout", help="timeout for action", type=int) + parser.add_argument("--testloop", help="run a loop of 'run', 'align' and 'wobble' tests", type=str) + parser.add_argument("--lifetest", help="run the gimbal life test", action='store_true') + parser.add_argument("--limp", help="disable position hold mode", action='store_true') + parser.add_argument("--nolimp", help="enable position hold mode", action='store_true') + parser.add_argument("--hard", help="enable low torque mode", action='store_true') + parser.add_argument("--soft", help="disable low torque mode", action='store_true') + if setup_home: + parser.add_argument("-j", "--jointcalibration", help="Calibrate joint angles", action='store_true') + parser.add_argument("-g", "--gyrocalibration", help="Calibrate gyros", action='store_true') + parser.add_argument("-a", "--accelcalibration", help="Calibrate accelerometers", action='store_true') + parser.add_argument("-x", "--staticcal", help="Calibrate all static home values", action='store_true') + if setup_factory_private: + parser.add_argument("--date", help="Setup assembly date", action='store_true') + parser.add_argument("--serialnumber", help="Setup gimbal serial number", type=int) + parser.add_argument("--factoryreset", help="Reset gimbal factory parameters to default", action='store_true') + parser.add_argument("--fullreset", help="Clear all gimbal parameters and program memory", action='store_true') + + args = parser.parse_args() + + # Open the serial port + port, link = setup_mavlink.open_comm(args.port) + print("Connecting via port %s" % port) + + if link is None: + print("Failed to open port %s" % port) + sys.exit(1) + + # Send a heartbeat first to wake up the interface, because mavlink + link.heartbeat_send(0, 0, 0, 0, 0) + + msg = setup_mavlink.get_any_gimbal_message(link) + if msg: + if setup_mavlink.is_bootloader_message(msg) and not args.file: + print("Gimbal in bootloader, only firmware loading is available") + sys.exit(0) + else: + # If the gimbal is using a different mavlink definitions version to + # what this script is using, try detecting the gimbal using a + # standard mavlink message that will not have changed. + # Standard mavlink heartbeats could be used, but are usually not + # propogated through the pixhawk in Solo + ver = setup_factory_pub.read_software_version(link, timeout=2) + if ver: + print("Possible mismatch in gimbal mavlink definitions, continuing anyway") + else: + print("No gimbal messages received, exiting.") + sys.exit(1) + + if args.file: + handle_file(args, link) + return + + if args.eraseapp: + result = start_bootloader(link) + if result == loader_results.NoResponse: + print("No response from gimbal, exiting.") + elif result == loader_results.InBoot: + print("Application already erased") + elif result == loader_results.Restarting: + print("Application erased") + return + + if args.calibrate: + runCalibration(link) + return + + if args.forcecal: + eraseCalibration(link) + # TODO: Check if this timeout is necessary + time.sleep(5) + runCalibration(link) + return + + if args.show: + params = setup_validate.show(link) + if params: + print(json.dumps(params, sort_keys=True, indent=4, separators=(',', ': '))) + else: + print("Error fetching parameters from gimbal") + return + + if args.save: + params = setup_validate.show(link) + if params: + if not os.path.isdir('logs'): + os.makedirs('logs') + + if params['serial_number'] != None and params['serial_number'] != '': + filePath = os.path.join('logs', '%s.json' % params['serial_number']) + with open(filePath, 'w') as f: + json.dump(params, f, sort_keys=True, indent=4, separators=(',', ': ')) + print("Gimbal prameters saved to %s" % filePath) + else: + print("Gimbal serial number not set, prameters not saved.") + else: + print("Error fetching parameters from gimbal") + return + + if args.validate: + print("Validating gimbal parameters...") + printValidation(link) + return + + if args.defaults: + setup_validate.restore_defaults(link) + print('Parameters restored to default values') + return + + if args.reboot: + if not setup_mavlink.reset_gimbal(link): + print('Failed to reboot') + return + + if args.erase: + eraseCalibration(link) + return + + if args.customgains == 1 or args.customgains == 0: + setup_param.set_use_custom_gains(link, args.customgains) + return + + if args.charging == 1 or args.charging == 0: + setup_param.charging(link, args.charging) + return + + if setup_run: + if args.run: + setup_run.runTest(link, 'run') + return + + if args.align: + setup_run.runTest(link, 'align') + return + + if args.wobble: + setup_run.runTest(link, 'wobble', timeout=args.timeout) + return + + if args.testloop: + if args.testloop in ['run', 'align', 'wobble']: + setup_run.runTestLoop(link, args.testloop, timeout=args.timeout) + return + else: + print("Unknown test: %s" % args.testloop) + + if args.lifetest: + setup_run.runLifeTest(link) + return + + if args.stop: + setup_run.stop(link) + return + + if args.limp: + setup_param.pos_hold_disable(link) + return + + if args.nolimp: + setup_param.pos_hold_enable(link) + return + + if args.hard: + setup_param.low_torque_mode(link, False) + return + + if args.soft: + setup_param.low_torque_mode(link, True) + return + + if setup_home: + if args.jointcalibration or args.staticcal: + print('Calibrating home position') + offsets = setup_home.calibrate_joints(link) + if offsets: + print('Calibrated home position') + else: + print('Failed to calibrate home position') + if not args.staticcal: + return + + if args.gyrocalibration or args.staticcal: + print('Calibrating gyro offsets') + offsets = setup_home.calibrate_gyro(link) + if offsets: + print('Calibrated gyro offsets') + else: + print('Failed to calibrate gyro offsets') + return + + if args.accelcalibration: + setup_home.calibrate_accel(link) + return + + if setup_factory_private: + if args.date: + timestamp = setup_factory_private.set_assembly_date(link) + print("Assembly time set to %s" % time.ctime(timestamp)) + return + + if args.serialnumber is not None: + serial = setup_factory_private.set_serial_number_3dr(link, args.serialnumber) + print("Serial number set to %s" % serial) + return + + if args.factoryreset: + serial = setup_factory_private.reset(link) + print("Facroty parameters cleared") + return + + if args.fullreset: + setup_factory_private.full_reset(link) + print("Full reset command sent") + return + + # Default command is to return the software version number + if firmware_git_tools: + os_git_command = firmware_git_tools.osGitCommand() + print("Tools version: %s"%(firmware_git_tools.gitIdentity(os_git_command))) + ver = setup_factory_pub.read_software_version(link, timeout=4) + if ver != None: + major, minor, rev = ver[0], ver[1], ver[2] + print("Software version: v%i.%i.%i" % (major, minor, rev)) + + serial_number = setup_factory_pub.get_serial_number(link) + if serial_number != None: + if serial_number == '': + print("Serial number: not set") + else: + print("Serial number: " + serial_number) + else: + print("Serial number: unknown") + + asm_time = setup_factory_pub.get_assembly_time(link) + if asm_time != None: + if asm_time > 0: + print("Assembled time: " + time.ctime(asm_time)) + else: + print("Assembly time: not set") + else: + print("Assembly time: unknown") + else: + print("Software version: unknown") + +if __name__ == '__main__': + command_interface() + sys.exit(0) diff --git a/gimbal/setup_comutation.py b/gimbal/setup_comutation.py new file mode 100644 index 0000000..359809d --- /dev/null +++ b/gimbal/setup_comutation.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python + +""" +Utility for loading firmware into the 3DR Gimbal. + +""" +import setup_mavlink, setup_param + +axis_enum = ['PITCH', 'ROLL', 'YAW'] +status_enum = ['in progress', 'succeeded', 'failed'] + +class Results: + Success, ParamFetchFailed, CalibrationExists, CommsFailed, PitchFailed, RollFailed, YawFailed = 'Success', 'ParamFetchFailed', 'CalibrationExists', 'CommsFailed', 'PitchFailed', 'RollFailed', 'YawFailed' + +def getAxisCalibrationValues(link): + values = { + 'pitch': {'slope': 0, 'intercept': 0}, + 'roll': {'slope': 0, 'intercept': 0}, + 'yaw': {'slope': 0, 'intercept': 0}, + } + for i in range(len(axis_enum)): + result = setup_param.getAxisCalibrationParam(link, axis_enum[i]) + if result == None: + return None + values[axis_enum[i].lower()]['slope'] = result[0] + values[axis_enum[i].lower()]['intercept'] = result[1] + return values + +def getAxisCalibrationParams(link): + pitch = setup_param.getAxisCalibrationParam(link, axis_enum[0]) + roll = setup_param.getAxisCalibrationParam(link, axis_enum[1]) + yaw = setup_param.getAxisCalibrationParam(link, axis_enum[2]) + if pitch == None or roll == None or yaw == None: + return None + else: + return pitch, roll, yaw + +def resetCalibration(link): + setup_param.clear_comutation_params(link) + setup_mavlink.reset_gimbal(link) + return True + +def calibrate(link, progressCallback=None): + # Check if a calibration exists, exit if it does + pitch, roll, yaw = getAxisCalibrationParams(link) + if pitch == None or roll == None or yaw == None: + return Results.ParamFetchFailed + elif (pitch[0] != 0 or pitch[1] != 0) and (roll[0] != 0 or roll[1] != 0) and (yaw[0] != 0 or yaw[1] != 0): + # Check if a calibration is in progress + result = setup_mavlink.getCalibrationProgress(link) + if result == None: + return Results.CalibrationExists + elif result[2] != status_enum[0]: + return Results.CalibrationExists + + calibratied_axes = 0 + axis_statuses = {'pitch': 'not started', 'roll': 'not started', 'yaw': 'not started'} + retries = 0 + lastProgress = None + while(calibratied_axes < 3): + setup_mavlink.requestCalibration(link) + result = setup_mavlink.getCalibrationProgress(link) + if result: + axis, progress, status = result[0], result[1], result[2] + elif retries > 10: + return Results.CommsFailed + else: + retries += 1 + continue + retries = 0 + + # Adjust number of calibrated axes if a previous calibration is already in progress + if calibratied_axes == 0 and axis == axis_enum[1]: + calibratied_axes == 1 + elif calibratied_axes == 0 and axis == axis_enum[2]: + calibratied_axes == 2 + + # Only run callback when progress has changed + if int(progress) != lastProgress: + progressCallback(axis, int(progress), status) + lastProgress = int(progress) + + if status != 'in progress': + axis_statuses[axis.lower()] = status + + # Don't continue calibration if an axis fails + if status == 'failed': + if axis == 'PITCH': + return Results.PitchFailed + elif axis == 'ROLL': + return Results.RollFailed + elif axis == 'YAW': + return Results.YawFailed + break + + calibratied_axes += 1 + return Results.Success diff --git a/gimbal/setup_factory_pub.py b/gimbal/setup_factory_pub.py new file mode 100644 index 0000000..1f1f3ca --- /dev/null +++ b/gimbal/setup_factory_pub.py @@ -0,0 +1,36 @@ +import struct +import setup_param + +def float_to_uint32(f): + return struct.unpack('I',struct.pack('f', f)) + +def float3_to_string12(f1, f2, f3): + return struct.unpack('12s',struct.pack('<3f', f1, f2, f3))[0] + +def get_assembly_time(link): + value = setup_param.fetch_param(link, "GMB_ASM_TIME", timeout=1) + if value: + return float_to_uint32(value.param_value) + return None + +def read_software_version(link, timeout=1): + msg = setup_param.fetch_param(link, "GMB_SWVER", timeout=timeout) + if not msg: + return None + else: + return float_to_bytes4(msg.param_value)[:-1] + +def get_serial_number(link): + ser_num_1 = setup_param.fetch_param(link, "GMB_SER_NUM_1", timeout=1) + ser_num_2 = setup_param.fetch_param(link, "GMB_SER_NUM_2", timeout=1) + ser_num_3 = setup_param.fetch_param(link, "GMB_SER_NUM_3", timeout=1) + if ser_num_1 != None and ser_num_2 != None and ser_num_3 != None: + serial_str = float3_to_string12(ser_num_1.param_value, ser_num_2.param_value, ser_num_3.param_value) + if serial_str.startswith('GB'): + return serial_str + else: + return '' + return None diff --git a/gimbal/setup_mavlink.py b/gimbal/setup_mavlink.py new file mode 100644 index 0000000..23978db --- /dev/null +++ b/gimbal/setup_mavlink.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python + +import os, sys, time, fnmatch +import serial.tools.list_ports +from pymavlink import mavutil +from pymavlink.mavutil import mavlink, mavserial, SerialPort +from pymavlink.dialects.v10 import ardupilotmega +from pymavlink.rotmat import Vector3 +import setup_comutation, setup_mavlink +import serial + +MAVLINK_SYSTEM_ID = 255 +MAVLINK_COMPONENT_ID = mavlink.MAV_COMP_ID_GIMBAL +TARGET_SYSTEM_ID = 1 +TARGET_COMPONENT_ID = mavlink.MAV_COMP_ID_GIMBAL + +DATA_TRANSMISSION_HANDSHAKE_SIZE_MAGIC = 0x42AA5542 + +def getSerialPorts(preferred_list=['*USB Serial*','*FTDI*']): + if os.name == 'nt': + ports = list(serial.tools.list_ports.comports()) + ret = [] + for name, desc, hwid in ports: + for preferred in preferred_list: + if fnmatch.fnmatch(desc, preferred) or fnmatch.fnmatch(hwid, preferred): + ret.append(SerialPort(name, description=desc, hwid=hwid)) + break + return ret + return mavutil.auto_detect_serial(preferred_list=preferred_list) + +def open_comm(port=None, baudrate=230400): + link = None + try: + if not port: + serial_list = getSerialPorts(preferred_list=['*USB Serial*','*FTDI*']) + if len(serial_list) >= 1: + port = serial_list[0].device + else: + port = '0.0.0.0:14550' + mavserial = mavutil.mavlink_connection(device=port, baud=baudrate) + link = mavlink.MAVLink(mavserial, MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID) + link.target_sysid = TARGET_SYSTEM_ID + link.target_compid = TARGET_COMPONENT_ID + except Exception: + pass + finally: + return (port, link) + +def wait_handshake(link, timeout=1, retries=1): + '''wait for a handshake so we know the target system IDs''' + for retries in range(retries): + msg = link.file.recv_match(type='DATA_TRANSMISSION_HANDSHAKE', blocking=True, timeout=timeout) + if msg and msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL: + return msg + return None + +def get_current_joint_angles(link): + while(True): + msg_gimbal = link.file.recv_match(type="GIMBAL_REPORT", blocking=True, timeout=2) + if msg_gimbal == None: + return None + else: + return Vector3([msg_gimbal.joint_roll, msg_gimbal.joint_el, msg_gimbal.joint_az]) + +def get_current_delta_angles(link): + while(True): + msg_gimbal = link.file.recv_match(type="GIMBAL_REPORT", blocking=True, timeout=2) + if msg_gimbal == None: + return None + else: + return Vector3([msg_gimbal.delta_angle_x, msg_gimbal.delta_angle_y, msg_gimbal.delta_angle_z]) + +def get_current_delta_velocity(link, timeout=1): + if not isinstance(link.file, mavserial): + print "accelerometer calibration requires a serial connection" + sys.exit(1) + link.file.port.flushInput() # clear any messages in the buffer, so we get a current one + while(True): + msg_gimbal = link.file.recv_match(type="GIMBAL_REPORT", blocking=True, timeout=timeout) + if msg_gimbal == None: + return None + else: + return Vector3([msg_gimbal.delta_velocity_x, msg_gimbal.delta_velocity_y, msg_gimbal.delta_velocity_z]) + +def get_gimbal_report(link, timeout=2): + msg_gimbal = link.file.recv_match(type="GIMBAL_REPORT", blocking=True, timeout=timeout) + return msg_gimbal + +def send_gimbal_control(link, rate): + link.gimbal_control_send(link.target_sysid, link.target_compid,rate.x,rate.y,rate.z) + +def reset_gimbal(link): + link.file.mav.command_long_send(link.target_sysid, link.target_compid, 42501, 0, 0, 0, 0, 0, 0, 0, 0) + result = link.file.recv_match(type="COMMAND_ACK", blocking=True, timeout=3) + if result: + # Sleep to allow the reset command to take + time.sleep(2) + # Wait for the gimbal to reset and begin comms again + return setup_mavlink.get_any_gimbal_message(link, timeout=5) + else: + return False + +def reset_into_bootloader(link): + return link.data_transmission_handshake_send(mavlink.MAVLINK_TYPE_UINT16_T, DATA_TRANSMISSION_HANDSHAKE_SIZE_MAGIC, 0, 0, 0, 0, 0) + +def exit_bootloader(link): + return link.data_transmission_handshake_send(mavlink.MAVLINK_TYPE_UINT16_T, 0, 0, 0, 0, 0, 0) + +def send_bootloader_data(link, sequence_number, data): + return link.encapsulated_data_send(sequence_number, data) + +def getCalibrationProgress(link): + while(True): + msg_progress = link.file.recv_match(type="COMMAND_LONG", blocking=True, timeout=1) + if msg_progress is None: + return None + if msg_progress.command == 42502: + break + + axis = setup_comutation.axis_enum[int(msg_progress.param1) - 1] + progress = int(msg_progress.param2) + status = setup_comutation.status_enum[int(msg_progress.param3)] + + return [axis, progress, status] + +def receive_home_offset_result(link): + return link.file.recv_match(type="COMMAND_ACK", blocking=True, timeout=3) + +def requestCalibration(link): + return link.file.mav.command_long_send(link.target_sysid, link.target_compid, 42503, 0, 0, 0, 0, 0, 0, 0, 0) + +def get_all(link, timeout=1): + msg = link.file.recv_match(blocking=True, timeout=timeout) + if msg != None: + if msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL: + return msg + return None + +def get_any_message(link, timeout=1): + return link.file.recv_match(blocking=True, timeout=timeout) + +def get_gimbal_message(link, timeout=2): + start_time = time.time() + while (time.time() - start_time) < timeout: + msg = link.file.recv_match(blocking=True, timeout=1) + if msg: + if msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL: + # Ignore the two types of bootloader messages + if msg.get_msgId() == mavlink.MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: + return False + if msg.get_msgId() == mavlink.MAVLINK_MSG_ID_HEARTBEAT: + continue + else: + return True + return False + +def wait_for_gimbal_message(link, timeout=5): + start_time = time.time() + while (time.time() - start_time) < timeout: + if get_gimbal_message(link, timeout=1): + return True + return None + +def get_any_gimbal_message(link, timeout=2): + start_time = time.time() + while (time.time() - start_time) < timeout: + msg = link.file.recv_match(blocking=True, timeout=1) + if msg: + if (msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL + and msg.get_msgId() != mavlink.MAVLINK_MSG_ID_HEARTBEAT): + return msg + return None + +def wait_for_any_gimbal_message(link, timeout=5): + start_time = time.time() + while (time.time() - start_time) < timeout: + if get_any_gimbal_message(link, timeout=1): + return True + return None + +def is_bootloader_message(msg): + if (msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL and + msg.get_msgId() == mavlink.MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE): + return True + return False + +if __name__ == '__main__': + import argparse, setup_home, setup_param + parser = argparse.ArgumentParser() + parser.add_argument("-p", "--port", help="Serial port or device used for MAVLink bootloading", default=None) + parser.add_argument("--jointstest", help="Run a number of joint calibrations", action='store_true') + parser.add_argument("--printjoints", help="Print out joint angles", action='store_true') + parser.add_argument("--printall", help="Print out all mavlink messages", action='store_true') + args = parser.parse_args() + + # Open the serial port + port, link = open_comm(args.port) + print("Connecting via port %s" % port) + + # Send a heartbeat first to wake up the interface + link.heartbeat_send(0, 0, 0, 0, 0) + + if args.jointstest: + print("Power On") + setup_param.enable_torques_message(link, enabled=False) + for i in range(5): + print setup_home.calibrate_joints(link) + + elif args.printjoints: + while True: + msg = get_gimbal_report(link) + print msg.joint_az, msg.joint_roll, msg.joint_el + + elif args.printall: + while True: + print get_any_message(link) + + else: + while True: + print time.time(), get_gimbal_message(link) diff --git a/gimbal/setup_param.py b/gimbal/setup_param.py new file mode 100644 index 0000000..e16a067 --- /dev/null +++ b/gimbal/setup_param.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python + +""" +Utility for loading firmware into the 3DR Gimbal. + +""" +import time +from pymavlink.mavparm import MAVParmDict +from pymavlink.dialects.v10.ardupilotmega import MAV_PARAM_TYPE_REAL32 +from pymavlink.rotmat import Vector3 +import setup_mavlink + +try: + import setup_factory_private +except ImportError: + setup_factory_private = None + +def fetch_param(link, param, timeout=2): + for i in range(timeout): + link.param_request_read_send(link.target_sysid, link.target_compid, param, -1) + msg = link.file.recv_match(type="PARAM_VALUE", blocking=True, timeout=1) + if msg: + return msg + return None + +def set_param(link, param_name, param_value): + parameters = MAVParmDict() + parameters.mavset(link.file, param_name, param_value, 3); + +def commit_to_flash(link): + # Commit the zeroed out calibration parameters to flash + link.param_set_send(link.target_sysid, 0, "GMB_FLASH", 69.0, MAV_PARAM_TYPE_REAL32) + # Sleep for two seconds as immedietly following commands will fail (while the C2000 is writing flash) + time.sleep(2) + +def load_param_file(pid, link): + parameters = MAVParmDict() + parameters.load(pid, 'GMB*', link.file, check=False) + commit_to_flash(link) + +def clear_comutation_params(link): + parameters = MAVParmDict() + + # Set all commutation calibration parameters to 0 + parameters.mavset(link.file, "GMB_YAW_SLOPE", 0.0, 3) + parameters.mavset(link.file, "GMB_YAW_ICEPT", 0.0, 3) + parameters.mavset(link.file, "GMB_ROLL_SLOPE", 0.0, 3) + parameters.mavset(link.file, "GMB_ROLL_ICEPT", 0.0, 3) + parameters.mavset(link.file, "GMB_PITCH_SLOPE", 0.0, 3) + parameters.mavset(link.file, "GMB_PITCH_ICEPT", 0.0, 3) + commit_to_flash(link) + +def set_offsets(link, kind, offsets): + if offsets and kind: + set_param(link, "GMB_OFF_" + kind + "_X", offsets.x) + set_param(link, "GMB_OFF_" + kind + "_Y", offsets.y) + set_param(link, "GMB_OFF_" + kind + "_Z", offsets.z) + commit_to_flash(link) + +def get_offsets(link, kind, timeout=2): + x = fetch_param(link, "GMB_OFF_" + kind + "_X", timeout=timeout) + y = fetch_param(link, "GMB_OFF_" + kind + "_Y", timeout=timeout) + z = fetch_param(link, "GMB_OFF_" + kind + "_Z", timeout=timeout) + if x == None or y == None or z == None: + return None + else: + return Vector3(x=x.param_value, y=y.param_value, z=z.param_value) + +def getAxisCalibrationParam(link, axis_enum): + icept = fetch_param(link, "GMB_" + axis_enum + "_ICEPT") + slope = fetch_param(link, "GMB_" + axis_enum + "_SLOPE") + if icept == None or slope == None: + return None + else: + return [icept.param_value, slope.param_value] + +def get_accel_params(link): + offsets_x = fetch_param(link, "GMB_OFF_ACC_X") + offsets_y = fetch_param(link, "GMB_OFF_ACC_Y") + offsets_z = fetch_param(link, "GMB_OFF_ACC_Z") + offset = Vector3(x=offsets_x.param_value, y=offsets_y.param_value, z=offsets_z.param_value) + + gains_x = fetch_param(link, "GMB_GN_ACC_X") + gains_y = fetch_param(link, "GMB_GN_ACC_Y") + gains_z = fetch_param(link, "GMB_GN_ACC_Z") + gain = Vector3(x=gains_x.param_value, y=gains_y.param_value, z=gains_z.param_value) + + alignments_x = fetch_param(link, "GMB_ALN_ACC_X") + alignments_y = fetch_param(link, "GMB_ALN_ACC_Y") + alignments_z = fetch_param(link, "GMB_ALN_ACC_Z") + alignment = Vector3(x=alignments_x.param_value, y=alignments_y.param_value, z=alignments_z.param_value) + + return offset, gain, alignment + +def set_accel_params(link, p, level): + parameters = MAVParmDict() + parameters.mavset(link.file, "GMB_OFF_ACC_X", p[0], 3) + parameters.mavset(link.file, "GMB_OFF_ACC_Y", p[1], 3) + parameters.mavset(link.file, "GMB_OFF_ACC_Z", p[2], 3) + parameters.mavset(link.file, "GMB_GN_ACC_X", p[3], 3) + parameters.mavset(link.file, "GMB_GN_ACC_Y", p[4], 3) + parameters.mavset(link.file, "GMB_GN_ACC_Z", p[5], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_X", level[0], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_Y", level[1], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_Z", level[2], 3) + commit_to_flash(link) + +def pos_hold_enable(link): + parameters = MAVParmDict() + parameters.mavset(link.file, "GMB_POS_HOLD", 1, 3) + +def pos_hold_disable(link): + parameters = MAVParmDict() + parameters.mavset(link.file, "GMB_POS_HOLD", 0, 3) + +def set_use_custom_gains(link, value): + current_param = fetch_param(link, "GMB_CUST_GAINS") + new_value = float(value) + if current_param: + if current_param.param_value != new_value: + set_param(link, "GMB_CUST_GAINS", new_value) + commit_to_flash(link) + else: + print("GMB_CUST_GAINS is already set to %0.1f" % new_value) + +def enable_torques_message(link, enabled=True): + if enabled: + value = 1.0 + else: + value = 0.0 + set_param(link, "GMB_SND_TORQUE", value) + +def low_torque_mode(link, enabled=True): + if enabled: + value = 5000 + else: + value = 0.0 + set_param(link, "GMB_MAX_TORQUE", value) + +def charging(link, enabled=True): + if enabled: + value = 1.0 + else: + value = 0.0 + set_param(link, "GMB_GP_CHARGE", value) + commit_to_flash(link) + +def restore_params(link, params): + parameters = MAVParmDict() + + # Accel params + parameters.mavset(link.file, "GMB_OFF_ACC_X", params['accel']['offset']['x'], 3) + parameters.mavset(link.file, "GMB_OFF_ACC_Y", params['accel']['offset']['y'], 3) + parameters.mavset(link.file, "GMB_OFF_ACC_Z", params['accel']['offset']['z'], 3) + parameters.mavset(link.file, "GMB_GN_ACC_X", params['accel']['gain']['x'], 3) + parameters.mavset(link.file, "GMB_GN_ACC_Y", params['accel']['gain']['y'], 3) + parameters.mavset(link.file, "GMB_GN_ACC_Z", params['accel']['gain']['z'], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_X", params['accel']['alignment']['x'], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_Y", params['accel']['alignment']['y'], 3) + parameters.mavset(link.file, "GMB_ALN_ACC_Z", params['accel']['alignment']['z'], 3) + + # Commutation params + parameters.mavset(link.file, "GMB_YAW_SLOPE", params['yaw']['slope'], 3) + parameters.mavset(link.file, "GMB_YAW_ICEPT", params['yaw']['icept'], 3) + parameters.mavset(link.file, "GMB_ROLL_SLOPE", params['roll']['slope'], 3) + parameters.mavset(link.file, "GMB_ROLL_ICEPT", params['roll']['icept'], 3) + parameters.mavset(link.file, "GMB_PITCH_SLOPE", params['pitch']['slope'], 3) + parameters.mavset(link.file, "GMB_PITCH_ICEPT", params['pitch']['icept'], 3) + + # Gyro + parameters.mavset(link.file, "GMB_OFF_GYRO_X", params['gyro']['x'], 3) + parameters.mavset(link.file, "GMB_OFF_GYRO_Y", params['gyro']['y'], 3) + parameters.mavset(link.file, "GMB_OFF_GYRO_Z", params['gyro']['z'], 3) + + # Joint + parameters.mavset(link.file, "GMB_OFF_JNT_X", params['gyro']['x'], 3) + parameters.mavset(link.file, "GMB_OFF_JNT_Y", params['gyro']['y'], 3) + parameters.mavset(link.file, "GMB_OFF_JNT_Z", params['gyro']['z'], 3) + + # Serial Number and Assembly Time + if setup_factory_private: + setup_factory_private.set_serial_number(link, params['serial_number'], commit=False) + set_param(link, "GMB_ASM_TIME", setup_factory_private.uint32_to_float(params['assembly_time'])) + + # Save the values + commit_to_flash(link) + + # Reset the gimbal + setup_mavlink.reset_gimbal(link) diff --git a/gimbal/setup_validate.py b/gimbal/setup_validate.py new file mode 100644 index 0000000..b5bb4c4 --- /dev/null +++ b/gimbal/setup_validate.py @@ -0,0 +1,278 @@ +import setup_comutation +import setup_factory_pub +import setup_param +from distutils.version import LooseVersion +from pymavlink.mavparm import MAVParmDict +from pymavlink.rotmat import Vector3 +import setup_mavlink +import math + +EXPECTED_VERSION = '0.26.0' + +EXPECTED_SERIAL_NUMBER_START = 'GB11A' +EXPETED_ASSEMBLY_DATE_MIN = 1434778800 # Sat Jun 20 02:40:00 BRT 2015 + +EXPECTED_PITCH_ICEPT_MAX = 0.35 +EXPECTED_PITCH_ICEPT_MIN = 0.05 +EXPECTED_PITCH_SLOPE_MAX = 0.14 +EXPECTED_PITCH_SLOPE_MIN = 0.12 +EXPECTED_ROLL_ICEPT_MIN = 0.35 +EXPECTED_ROLL_ICEPT_MAX = 0.53 +EXPECTED_ROLL_SLOPE_MAX = 0.15 +EXPECTED_ROLL_SLOPE_MIN = 0.11 +EXPECTED_YAW_ICEPT_MAX = 0.60 +EXPECTED_YAW_ICEPT_MIN = 0.40 +EXPECTED_YAW_SLOPE_MAX = 0.13 +EXPECTED_YAW_SLOPE_MIN = 0.09 + +EXPECTED_JOINT_X_MAX = 0.20 +EXPECTED_JOINT_X_MIN = -0.14 +EXPECTED_JOINT_Y_MAX = 0.20 +EXPECTED_JOINT_Y_MIN = -0.07 +EXPECTED_JOINT_Z_MAX = 0.18 +EXPECTED_JOINT_Z_MIN = -0.18 + +EXPECTED_ACC_GAIN_MIN = 0.95 +EXPECTED_ACC_GAIN_MAX = 1.05 + +EXPECTED_ACC_ALIGN_X_MIN = math.radians(-5) +EXPECTED_ACC_ALIGN_X_MAX = math.radians(5) +EXPECTED_ACC_ALIGN_Y_MIN = math.radians(-5) +EXPECTED_ACC_ALIGN_Y_MAX = math.radians(5) +EXPECTED_ACC_ALIGN_Z_MIN = math.radians(-5) +EXPECTED_ACC_ALIGN_Z_MAX = math.radians(5) + +EXPECTED_ACC_OFFSET_X_MIN = -2.0 +EXPECTED_ACC_OFFSET_X_MAX = 2.0 +EXPECTED_ACC_OFFSET_Y_MIN = -2.0 +EXPECTED_ACC_OFFSET_Y_MAX = 2.0 +EXPECTED_ACC_OFFSET_Z_MIN = -2.0 +EXPECTED_ACC_OFFSET_Z_MAX = 2.0 + +EXPECTED_GYRO = 0.07 + +class Results: + Pass, Fail, Error = 'pass', 'fail', 'error' + +def show(link): + swver = setup_factory_pub.read_software_version(link, timeout=2) + if swver != None: + major, minor, rev = int(swver[0]), int(swver[1]), int(swver[2]) + if (major == 0 and minor >= 18) or major > 0: + serial_number = setup_factory_pub.get_serial_number(link) + assembly_time = setup_factory_pub.get_assembly_time(link) + else: + serial_number = '' + assembly_time = '' + pitch_com, roll_com, yaw_com = setup_comutation.getAxisCalibrationParams(link) + joint = setup_param.get_offsets(link, 'JNT') + gyro = setup_param.get_offsets(link, 'GYRO') + accel_offset, accel_gain, accel_alignment = setup_param.get_accel_params(link) + k_rate = setup_param.fetch_param(link, "GMB_K_RATE") + + if swver != None and serial_number != None and assembly_time != None and pitch_com != None and roll_com != None and yaw_com != None and isinstance(joint, Vector3) and isinstance(gyro, Vector3) and isinstance(accel_offset, Vector3) and isinstance(accel_gain, Vector3) and isinstance(accel_alignment, Vector3) and k_rate != None: + params = { + 'version': swver, + 'serial_number': serial_number, + 'assembly_time': assembly_time, + 'pitch': { + 'icept': pitch_com[0], + 'slope': pitch_com[1] + }, + 'roll': { + 'icept': roll_com[0], + 'slope': roll_com[1] + }, + 'yaw': { + 'icept': yaw_com[0], + 'slope': yaw_com[1] + }, + 'joint': { + 'x': joint.x, + 'y': joint.y, + 'z': joint.z + }, + 'gyro': { + 'x': gyro.x, + 'y': gyro.y, + 'z': gyro.z + }, + 'accel': { + 'offset': { + 'x': accel_offset.x, + 'y': accel_offset.y, + 'z': accel_offset.z + }, + 'gain': { + 'x': accel_gain.x, + 'y': accel_gain.y, + 'z': accel_gain.z + }, + 'alignment': { + 'x': accel_alignment.x, + 'y': accel_alignment.y, + 'z': accel_alignment.z + } + }, + 'k_rate': k_rate.param_value, + 'validation': { + 'version': validate_version(link, swver=swver), + 'serial': validate_serial_number(link, serial_number=serial_number), + 'date': validate_date(link, assembly_time=assembly_time), + 'commutation': { + 'pitch': validate_comutation_axis_value('pitch', pitch_com), + 'roll': validate_comutation_axis_value('roll', roll_com), + 'yaw': validate_comutation_axis_value('yaw', yaw_com) + }, + 'joints': validate_joints(link, joint=joint), + 'gyros': validate_gyros(link, gyro=gyro), + 'accels': validate_accelerometers(link, gain=accel_gain, offset=accel_offset, alignment=accel_alignment) + } + } + return params + else: + return None + +def validate_version(link, swver=None): + if not swver: + swver = setup_factory_pub.read_software_version(link, timeout=2) + if not swver: + return Results.Error + ver = LooseVersion("%i.%i.%i" % (swver[0], swver[1], swver[2])) + ver_expected = LooseVersion(EXPECTED_VERSION) + if ver >= ver_expected: + return Results.Pass + else: + return Results.Fail + +def validate_comutation_axis_value(axis, values): + if axis == 'pitch': + valid = validate_comutation_axis(None, values, EXPECTED_PITCH_ICEPT_MAX, EXPECTED_PITCH_ICEPT_MIN, EXPECTED_PITCH_SLOPE_MAX, EXPECTED_PITCH_SLOPE_MIN) + elif axis == 'roll': + valid = validate_comutation_axis(None, values, EXPECTED_ROLL_ICEPT_MAX, EXPECTED_ROLL_ICEPT_MIN, EXPECTED_ROLL_SLOPE_MAX, EXPECTED_ROLL_SLOPE_MIN) + elif axis == 'yaw': + valid = validate_comutation_axis(None, values, EXPECTED_YAW_ICEPT_MAX, EXPECTED_YAW_ICEPT_MIN, EXPECTED_YAW_SLOPE_MAX, EXPECTED_YAW_SLOPE_MIN) + else: + return Results.Error + + if valid: + return Results.Pass + else: + return Results.Fail + +def validate_comutation_axis(link, axis, i_max, i_min, s_max, s_min): + icept = axis[0] + slope = axis[1] + if (icept == 0) or (i_min >= icept) or (i_max <= icept): + return False + elif (icept == 0) or (s_min >= slope) or (s_max <= slope): + return False + else: + return True + +def validate_comutation(link, pitch_com=None, roll_com=None, yaw_com=None): + "The acceptable range values where collected from the batch of DVT1 gimbals" + if pitch_com == None or roll_com == None or yaw_com == None: + pitch_com, roll_com, yaw_com = setup_comutation.getAxisCalibrationParams(link) + if pitch_com == None or roll_com == None or yaw_com == None: + return Results.Error + if (validate_comutation_axis(link, pitch_com, EXPECTED_PITCH_ICEPT_MAX, EXPECTED_PITCH_ICEPT_MIN, EXPECTED_PITCH_SLOPE_MAX, EXPECTED_PITCH_SLOPE_MIN) and + validate_comutation_axis(link, roll_com, EXPECTED_ROLL_ICEPT_MAX, EXPECTED_ROLL_ICEPT_MIN, EXPECTED_ROLL_SLOPE_MAX, EXPECTED_ROLL_SLOPE_MIN) and + validate_comutation_axis(link, yaw_com, EXPECTED_YAW_ICEPT_MAX, EXPECTED_YAW_ICEPT_MIN, EXPECTED_YAW_SLOPE_MAX, EXPECTED_YAW_SLOPE_MIN)): + return Results.Pass + else: + return Results.Fail + +def validate_joints(link, joint=None): + "The acceptable range values where collected from the batch of DVT1 gimbals" + if not isinstance(joint, Vector3): + joint = setup_param.get_offsets(link, 'JNT') + if not isinstance(joint, Vector3): + return Results.Error + if ((joint.x <= EXPECTED_JOINT_X_MAX) and (joint.x >= EXPECTED_JOINT_X_MIN) and (joint.x != 0) and + (joint.y <= EXPECTED_JOINT_Y_MAX) and (joint.y >= EXPECTED_JOINT_Y_MIN) and (joint.y != 0) and + (joint.z <= EXPECTED_JOINT_Z_MAX) and (joint.z >= EXPECTED_JOINT_Z_MIN) and (joint.z != 0)): + return Results.Pass + else: + return Results.Fail + +def validate_gyros(link, gyro=None): + "The acceptable range values where collected from the batch of DVT1 gimbals" + if not isinstance(gyro, Vector3): + gyro = setup_param.get_offsets(link, 'GYRO') + if not isinstance(gyro, Vector3): + return Results.Error + if ((gyro.x <= EXPECTED_GYRO) and (gyro.x >= -EXPECTED_GYRO) and (gyro.x != 0) and + (gyro.y <= EXPECTED_GYRO) and (gyro.y >= -EXPECTED_GYRO) and (gyro.y != 0) and + (gyro.z <= EXPECTED_GYRO) and (gyro.z >= -EXPECTED_GYRO) and (gyro.z != 0)): + return Results.Pass + else: + return Results.Fail + +def validate_accelerometers(link, offset=None, gain=None, alignment=None): + "Since there is no accelerometer cal yet, just check if the values are zeroed" + if not isinstance(offset, Vector3) or not isinstance(gain, Vector3) or not isinstance(alignment, Vector3): + offset, gain, alignment = setup_param.get_accel_params(link) + if not isinstance(offset, Vector3) or not isinstance(gain, Vector3) or not isinstance(alignment, Vector3): + return Results.Error + if ((offset.x >= EXPECTED_ACC_OFFSET_X_MIN) and (offset.x <= EXPECTED_ACC_OFFSET_X_MAX) and + (offset.y >= EXPECTED_ACC_OFFSET_Y_MIN) and (offset.y <= EXPECTED_ACC_OFFSET_Y_MAX) and + (offset.z >= EXPECTED_ACC_OFFSET_Z_MIN) and (offset.z <= EXPECTED_ACC_OFFSET_Z_MAX) and + (alignment.x >= EXPECTED_ACC_ALIGN_X_MIN) and (alignment.x <= EXPECTED_ACC_ALIGN_X_MAX) and + (alignment.y >= EXPECTED_ACC_ALIGN_Y_MIN) and (alignment.y <= EXPECTED_ACC_ALIGN_Y_MAX) and + (alignment.z >= EXPECTED_ACC_ALIGN_Z_MIN) and (alignment.z <= EXPECTED_ACC_ALIGN_Z_MAX) and + (gain.x >= EXPECTED_ACC_GAIN_MIN) and (gain.x <= EXPECTED_ACC_GAIN_MAX) and + (gain.y >= EXPECTED_ACC_GAIN_MIN) and (gain.y <= EXPECTED_ACC_GAIN_MAX) and + (gain.z >= EXPECTED_ACC_GAIN_MIN) and (gain.z <= EXPECTED_ACC_GAIN_MAX)): + return Results.Pass + else: + return Results.Fail + +def validate_gains(link): + custom_gains = setup_param.fetch_param(link, "GMB_CUST_GAINS") + if custom_gains is None: + return Results.Error + elif custom_gains.param_value == 0.0: + return Results.Pass + else: + return Results.Fail + +def validate_date(link, assembly_time=None): + if assembly_time == None: + assembly_time = setup_factory_pub.get_assembly_time(link) + if assembly_time == None: + return Results.Error + elif assembly_time > EXPETED_ASSEMBLY_DATE_MIN: + return Results.Pass + else: + return Results.Fail + +def validate_serial_number(link, serial_number=None): + if serial_number == None: + serial_number = setup_factory_pub.get_serial_number(link) + if serial_number == None: + return Results.Error + elif serial_number.startswith(EXPECTED_SERIAL_NUMBER_START): + return Results.Pass + else: + return Results.Fail + +def validate(link): + validation = { + 'version': validate_version(link), + 'serial': validate_serial_number(link), + 'date': validate_date(link), + 'commutation': validate_comutation(link), + 'joints': validate_joints(link), + 'gyros': validate_gyros(link), + 'accels': validate_accelerometers(link), + 'gains': validate_gains(link) + } + return validation + +def restore_defaults(link): + parameters = MAVParmDict() + parameters.mavset(link.file, "GMB_CUST_GAINS", 0.0); # do not use custom gains for default values + setup_param.commit_to_flash(link) + setup_mavlink.reset_gimbal(link) + return True diff --git a/init/3dr_rotate b/init/3dr_rotate new file mode 100755 index 0000000..fcd4210 --- /dev/null +++ b/init/3dr_rotate @@ -0,0 +1,41 @@ +#!/bin/sh + +echo "rotating 3dr logs" + +if [ -f /usr/sbin/logrotate ]; then + logrotate -f /etc/logrotate-sololink.conf -s /dev/null +fi + +if [ "`hostname`" == "3dr_solo" ]; then + + # Set links to shotlogs for backwards compatibility (external fetchers). + # Use of the three-digit-number names should be considered deprecated. + # + # shotlog.000.log -> shotlog.log + # shotlog.001.log -> shotlog.log.1 + # shotlog.002.log -> shotlog.log.2 + # : + # shotlog.019.log -> shotlog.log.19 + # + # This should be done after the rotate above, so the files have the names they + # will have for this run. + + cd /log + + # on first boot, shotlog.log is not there + # (don't know why logrotate is not creating it empty) + ln -sf shotlog.log shotlog.000.log + + for n in 1 2 3 4 5 6 7 8 9; do + if [ -f shotlog.log.$n ]; then + ln -sf shotlog.log.$n shotlog.00$n.log + fi + done + + for n in 10 11 12 13 14 15 16 17 18 19; do + if [ -f shotlog.log.$n ]; then + ln -sf shotlog.log.$n shotlog.0$n.log + fi + done + +fi diff --git a/init/clock_sync b/init/clock_sync new file mode 100755 index 0000000..9b5d04f --- /dev/null +++ b/init/clock_sync @@ -0,0 +1,27 @@ +#!/bin/sh + +# Normally, /etc/timestamp is update in a shutdown script. +# +# However, on Solo, we do not shut down; power just disappears. +# +# The goal of this script is to make time move forward, more-or-less, between +# runs, i.e. all logs from one run should have timestamps greater than those +# in logs from a previous run. /etc/timestamp only has minute-resolution, so +# there can be overlap of up to about a minute between the very end of one +# run and the very beginning of the next run. +# +# This is started by init and runs as long as the system is up. +# +# This only runs on Solo; Artoo is expected to get a power-button shutdown. +# +# Implementation note: Since power just disappears, we try to update +# /etc/timestamp in a way such that it is always has valid contents. If we +# just > into it, there may be a short time period when it is empty or does +# not exist. The goal of writing a temp file, then doing a move, is to reduce +# or eliminate the time window when it does not exist. + +while true; do + date -u +%4Y%2m%2d%2H%2M > /etc/timestamp.new + mv /etc/timestamp.new /etc/timestamp + sleep 10 +done diff --git a/init/pixhawk b/init/pixhawk new file mode 100755 index 0000000..b0bc943 --- /dev/null +++ b/init/pixhawk @@ -0,0 +1,3 @@ +#!/bin/sh +echo "initializing pixhawk interface..." +/usr/bin/pixhawk.py -i diff --git a/init/shutdownArtoo.sh b/init/shutdownArtoo.sh new file mode 100755 index 0000000..efe1a94 --- /dev/null +++ b/init/shutdownArtoo.sh @@ -0,0 +1,29 @@ +#!/bin/sh + +IMX_POWER=21 +BOARD_POWER=19 +GPIODIR=/sys/class/gpio + +#sync, shutdown hard drives +/sbin/halt -w -h + +echo $BOARD_POWER >> $GPIODIR/export +echo out >> $GPIODIR/gpio$BOARD_POWER/direction +echo 0 >> $GPIODIR/gpio$BOARD_POWER/value + +echo $IMX_POWER >> $GPIODIR/export +echo out >> $GPIODIR/gpio$IMX_POWER/direction +echo 0 >> $GPIODIR/gpio$IMX_POWER/value + +# +# the stm32 is ready to shut down before we are, +# but we rely on it to wait for us, so that the entire +# system can agree that we're powered down. +# +# rough timeline: +# - stm32 gets hold-pwr-button-to-shutdown gesture, sends please-shutdown msg +# - imx6 starts shutting down, stm32 shows progress spinner & keeps power to imx6 enabled +# - stm32 waits a hard coded duration for imx6 shutdown to complete. would prefer to timeout on a heartbeat, or similar. +# - stm32 ensures power button has been released long enough for rc circuit to discharge +# - stm32 disables power to the imx6 and either turns off if charger is not there, or goes to sleep +# \ No newline at end of file diff --git a/init/startwd b/init/startwd new file mode 100755 index 0000000..0de0b4f --- /dev/null +++ b/init/startwd @@ -0,0 +1,3 @@ +#!/bin/sh +echo "starting watchdog..." +/usr/bin/wdog -t 15 diff --git a/init/updateGimbal.sh b/init/updateGimbal.sh new file mode 100755 index 0000000..edd9e19 --- /dev/null +++ b/init/updateGimbal.sh @@ -0,0 +1,2 @@ +#!/bin/bash +/usr/bin/loadGimbal.py diff --git a/net/etc/init.d/hostapd b/net/etc/init.d/hostapd new file mode 100755 index 0000000..0889cb3 --- /dev/null +++ b/net/etc/init.d/hostapd @@ -0,0 +1,84 @@ +#!/bin/sh +DAEMON=/usr/sbin/hostapd +NAME=hostapd +DESC="HOSTAP Daemon" +HOSTAPD_CONF="/etc/hostapd.conf" +HOSTAPD_BACK="/etc/hostapd.back" +ARGS="/etc/hostapd.conf -B" +SOLO_CONF="/etc/sololink.conf" + +test -f $DAEMON || exit 0 + +set -e + +# Return true if parameter ($2) in config file ($1) is "True" +isEnabled() { + grep -i -q "[[:space:]]*$2[[:space:]]*=[[:space:]]*True" $1 +} + +# Return true if the SSID in hostapd.conf is still the default +isDefault() { + grep -i -q "^ssid=SoloLink_Default" $1 +} + +#Return the country code we should be using +getCountryCode() { + mkdir -p /tmp/bootmnt + mount /dev/mmcblk0p1 /tmp/bootmnt -o ro + if [ ! -e /tmp/bootmnt/.reg ]; then + echo "US" + else + cat /tmp/bootmnt/.reg + fi + umount /tmp/bootmnt + rm -rf /tmp/bootmnt +} + +case "$1" in + start) + # If the 3dr config file does not exist, or if it does and ApEnable + # is True, start hostapd + if [ ! -f $SOLO_CONF ] || isEnabled $SOLO_CONF ApEnable; then + echo -n "Starting $DESC: " + # back up hostapd.conf + cp ${HOSTAPD_CONF} ${HOSTAPD_BACK} + md5sum ${HOSTAPD_BACK} > ${HOSTAPD_BACK}.md5 + sync + # Unique-ize the SSID if it has not been done + if isDefault ${HOSTAPD_CONF}; then + # update SSID + /usr/bin/hostapdconfig.py --ssid SoloLink_ --ssidmac wlan0-ap + # initialize regulatory domain + /usr/bin/hostapdconfig.py --country `getCountryCode` + fi + # set channel to that of wlan0 if it is associated + /usr/bin/hostapdconfig.py --channel wlan0 + md5sum ${HOSTAPD_CONF} > ${HOSTAPD_CONF}.md5 + sync + rm ${HOSTAPD_BACK} ${HOSTAPD_BACK}.md5 + sync + start-stop-daemon -S -x $DAEMON -- $ARGS + echo "$NAME." + fi + ;; + stop) + echo -n "Stopping $DESC: " + start-stop-daemon -K -x $DAEMON + echo "$NAME." + ;; + restart) + $0 stop + $0 start + ;; + reload) + echo -n "Reloading $DESC: " + killall -HUP $(basename ${DAEMON}) + echo "$NAME." + ;; + *) + echo "Usage: $0 {start|stop|restart|reload}" + exit 1 + ;; +esac + +exit 0 diff --git a/net/etc/init.d/netinit b/net/etc/init.d/netinit new file mode 100755 index 0000000..93dd308 --- /dev/null +++ b/net/etc/init.d/netinit @@ -0,0 +1,25 @@ +#!/bin/sh + +NAME=wlan0-ap +DESC="AP Interface" +SOLO_CONF="/etc/sololink.conf" + +# Return true if parameter ($2) in config file ($1) is "True" +isEnabled() { + grep -i -q "[[:space:]]*$2[[:space:]]*=[[:space:]]*True" $1 +} + +# wlan0 exists only if the PCIe phy initialized +# wlan0-ap never exists at this point (this is where we create it) +if [ -e /sys/class/net/wlan0 ]; then + # If the 3dr config file does not exist, or if it does and ApEnable is True, + # set up the wlan0-ap interface + if [ ! -f $SOLO_CONF ] || isEnabled $SOLO_CONF ApEnable; then + echo -n "Initializing $DESC: " + iw phy phy0 interface add $NAME type __ap + ip link set dev $NAME address `/usr/bin/getmaclocal.py wlan0` + echo "$NAME." + fi +else + echo "wlan0 does not exist" +fi diff --git a/net/etc/init.d/networking b/net/etc/init.d/networking new file mode 100755 index 0000000..450c777 --- /dev/null +++ b/net/etc/init.d/networking @@ -0,0 +1,117 @@ +#!/bin/sh -e +### BEGIN INIT INFO +# Provides: networking +# Required-Start: mountvirtfs $local_fs +# Required-Stop: $local_fs +# Should-Start: ifupdown +# Should-Stop: ifupdown +# Default-Start: S +# Default-Stop: 0 6 +# Short-Description: Raise network interfaces. +### END INIT INFO + +PATH="/usr/local/sbin:/usr/local/bin:/sbin:/bin:/usr/sbin:/usr/bin" +CONF="/etc/sololink.conf" + +[ -x /sbin/ifup ] || exit 0 + +check_network_file_systems() { + [ -e /proc/mounts ] || return 0 + + if [ -e /etc/iscsi/iscsi.initramfs ]; then + echo "not deconfiguring network interfaces: iSCSI root is mounted." + exit 0 + fi + + exec 9<&0 < /proc/mounts + while read DEV MTPT FSTYPE REST; do + case $DEV in + /dev/nbd*|/dev/nd[a-z]*|/dev/etherd/e*) + echo "not deconfiguring network interfaces: network devices still mounted." + exit 0 + ;; + esac + case $FSTYPE in + nfs|nfs4|smbfs|ncp|ncpfs|cifs|coda|ocfs2|gfs|pvfs|pvfs2|fuse.httpfs|fuse.curlftpfs) + echo "not deconfiguring network interfaces: network file systems still mounted." + exit 0 + ;; + esac + done + exec 0<&9 9<&- +} + +check_network_swap() { + [ -e /proc/swaps ] || return 0 + + exec 9<&0 < /proc/swaps + while read DEV MTPT FSTYPE REST; do + case $DEV in + /dev/nbd*|/dev/nd[a-z]*|/dev/etherd/e*) + echo "not deconfiguring network interfaces: network swap still mounted." + exit 0 + ;; + esac + done + exec 0<&9 9<&- +} + +isEnabled() { + grep -i -q "[[:space:]]*$2[[:space:]]*=[[:space:]]*True" $1 +} + +case "$1" in +start) + echo -n "Configuring network interfaces... " + sysctl -e -p /etc/sysctl.conf >/dev/null 2>&1 + # Since we might modprobe -r ath9k in a networking shutdown, + # be sure to re-modprobe it at a startup + modprobe ath9k + ifup -a + if [ -e /sys/class/net/wlan0 ]; then + # if the config file exists and StationEnable is True, bring up wlan0 + if [ -f $CONF ] && isEnabled $CONF StationEnable; then + ifup wlan0 + fi + else + # this should exist on both solo and controller + echo -n "(wlan0 does not exist!) " + fi + if [ -e /sys/class/net/wlan0-ap ]; then + # if the config file does not exist, or if it does and ApEnable is + # True, bring up wlan0-ap + if [ ! -f $CONF ] || isEnabled $CONF ApEnable; then + ifup wlan0-ap + fi + fi + echo "done." + ;; + +stop) + check_network_file_systems + check_network_swap + + echo -n "Deconfiguring network interfaces... " + ifdown wlan0 &> /dev/null + ifdown wlan0-ap &> /dev/null + ifdown -a + echo "done." + ;; + +force-reload|restart) + echo "Running $0 $1 is deprecated because it may not enable again some interfaces" + echo "Reconfiguring network interfaces... " + # this will restart only the "auto" interfaces (not wlan0 or wlan0-ap) + ifdown -a || true + ifup -a + echo "done." + ;; + +*) + echo "Usage: /etc/init.d/networking {start|stop}" + exit 1 + ;; +esac + +exit 0 + diff --git a/net/usr/bin/.gitignore b/net/usr/bin/.gitignore new file mode 100644 index 0000000..0d20b64 --- /dev/null +++ b/net/usr/bin/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/net/usr/bin/clock.py b/net/usr/bin/clock.py new file mode 100755 index 0000000..86e29f1 --- /dev/null +++ b/net/usr/bin/clock.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +# Python interface into the posix clock functions +# clock_gettime(), clock_settime() +# +# Motivation is to get access to CLOCK_MONOTONIC, so flight code can not care +# about jumps in system time (in which case CLOCK_REALTIME and python's +# datetime jump). + +import ctypes +import os + +CLOCK_REALTIME = 0 +CLOCK_MONOTONIC = 1 +CLOCK_PROCESS_CPUTIME_ID = 2 +CLOCK_THREAD_CPUTIME_ID = 3 +CLOCK_MONOTONIC_RAW = 4 +CLOCK_REALTIME_COARSE = 5 +CLOCK_MONOTONIC_COARSE = 6 +CLOCK_BOOTTIME = 7 +CLOCK_REALTIME_ALARM = 8 +CLOCK_BOOTTIME_ALARM = 9 + +class timespec(ctypes.Structure): + _fields_ = [ + ("tv_sec", ctypes.c_long), + ("tv_nsec", ctypes.c_long) + ] + +librt = ctypes.CDLL('librt.so.1', use_errno=True) + +clock_gettime = librt.clock_gettime +clock_gettime.argtypes = [ctypes.c_int, ctypes.POINTER(timespec)] + +clock_settime = librt.clock_settime +clock_settime.argtypes = [ctypes.c_int, ctypes.POINTER(timespec)] + +def gettime(clock_id): + t = timespec() + if clock_gettime(clock_id, ctypes.pointer(t)) != 0: + errno_ = ctypes.get_errno() + raise OSError(errno_, os.strerror(errno_)) + return t + +def gettime_us(clock_id): + t = gettime(clock_id) + return long(t.tv_sec * 1000000 + (t.tv_nsec + 500) / 1000) + +def settime(clock_id, t): + if clock_settime(clock_id, ctypes.pointer(t)) != 0: + errno_ = ctypes.get_errno() + raise OSError(errno_, os.strerror(errno_)) + +def settime_us(clock_id, us): + t = timespec(us/1000000, (us%1000000) * 1000) + if clock_settime(clock_id, ctypes.pointer(t)) != 0: + errno_ = ctypes.get_errno() + raise OSError(errno_, os.strerror(errno_)) + +def test(do_set): + rt = gettime(CLOCK_REALTIME) + print "REALTIME: %10d.%09d" % (rt.tv_sec, rt.tv_nsec) + mt = gettime(CLOCK_MONOTONIC) + print "MONOTONIC: %10d.%09d" % (mt.tv_sec, mt.tv_nsec) + if do_set: + # WARNING: this sets the time to zero (1/1/1970) + print "set time..." + os.system("date --set=\"@0\"") + rt = gettime(CLOCK_REALTIME) + print "REALTIME: %10d.%09d" % (rt.tv_sec, rt.tv_nsec) + mt = gettime(CLOCK_MONOTONIC) + print "MONOTONIC: %10d.%09d" % (mt.tv_sec, mt.tv_nsec) + +if __name__ == "__main__": + test(False) # True will set your system time to zero diff --git a/net/usr/bin/configfile.py b/net/usr/bin/configfile.py new file mode 100755 index 0000000..ae07677 --- /dev/null +++ b/net/usr/bin/configfile.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Get and set parameters in a configuration file (e.g. /etc/hostapd.conf) + +import re +import os + +# Assumptions (all derived from looking at a sample hostapd.conf file): +# +# A valid parameter name is [a-zA-Z0-9_]+ +# +# A valid parameter value is everything after the '=' to the end of the line, +# whitespace trimmed on the ends. Can contain whitespace, ., :, etc. Can't +# handle a comment at the end of a setting; it will be taken as part of the +# setting. +# +# A "parameter setting" is: +# - optional [\t ]* +# - valid parameter name +# - optional [\t ]* +# - '=' +# - valid parameter value +# +# A "commented out parameter setting" is: +# - optional [\t ]* +# - '#' +# - "parameter setting" + +def paramGet(configFileName, paramName): + ''' + Read parameter setting from config file + ''' + configFile = open(configFileName) # IOError + + # process line-by-line + for line in configFile: + # re.match only works at beginning of line (we want that) + m = re.match('[\t ]*' + paramName + '[\t ]*=[\t ]*(.*?)[\t ]*\n', line) + if m: + return m.group(1) + + # not found + return None + + +def paramSet(configFileName, paramName, paramValue): + ''' + Set parameter in config file + ''' + + configFileNameNew = configFileName + '.new' + + # We generally make two passes through the file: + # + # Pass 1: + # If we find the parameter getting set (uncommented), we delete that + # line and replace it with a new line with the new setting. Copy the + # rest of the file then return (do not do pass 2). + # + # Pass 2: + # Otherwise, find the first line with a setting for the parameter + # commented out. Insert a new line with the parameter setting, but + # leave the commented out setting. Copy the rest of the file then + # return. If the parameter is not found (commented out), append the + # setting at the end of the file. + + paramSet = False + + # pass 1: process line-by-line + configFile = open(configFileName) # IOError + configFileNew = open(configFileNameNew, 'w') # IOError + for line in configFile: + if not paramSet: + # .match only works at beginning of line (we want that) + # optional tabs/spaces, 'param=', setting + m = re.match('[\t ]*' + paramName + '[\t ]*=[\t ]*(.*?)[\t ]*\n', line) + if m: + # found an uncommented setting for the parameter + # replace with the new setting + line = paramName + '=' + str(paramValue) + '\n' + paramSet = True + configFileNew.write(line) + configFileNew.close() + configFile.close() + + # pass 1 done: if we set the parameter, then we are done. + # If we did not set the parameter, do pass #2. + if not paramSet: + configFile = open(configFileName) # IOError + configFileNew = open(configFileNameNew, 'w') # IOError + for line in configFile: + if not paramSet: + # .match only works at beginning of line (we want that) + # '#', optional tabs/spaces, 'param=', setting + m = re.match('[\t ]*#[\t ]*' + paramName + '[\t ]*=[\t ]*(.*?)[\t ]*\n', line) + if m: + # found a commented setting for the parameter + # insert a line with the new setting + configFileNew.write(paramName + '=' + str(paramValue) + '\n') + paramSet = True + configFileNew.write(line) + # append setting to end of file if not yet set + if not paramSet: + configFileNew.write(paramName + '=' + str(paramValue) + '\n') + paramSet = True + configFileNew.close() + configFile.close() + + # replace old file with updated one + os.rename(configFileNameNew, configFileName) diff --git a/net/usr/bin/getmaclocal.py b/net/usr/bin/getmaclocal.py new file mode 100755 index 0000000..180b037 --- /dev/null +++ b/net/usr/bin/getmaclocal.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python + +import sys +import ip + +if len(sys.argv) < 2: + print 'Usage: ' + sys.argv[0] + ' ' + sys.exit(1) + +oldMac = ip.getMac(sys.argv[1]) +if oldMac is None: + print 'ERROR getting MAC address for ' + sys.argv[1] + sys.exit(1) + +newMac = ip.macSetLocal(oldMac) +if newMac is None: + print 'ERROR getting local MAC address for ' + oldMac + sys.exit(1) + +print newMac + +sys.exit(0) diff --git a/net/usr/bin/hostapdconfig.py b/net/usr/bin/hostapdconfig.py new file mode 100755 index 0000000..1bbe1d4 --- /dev/null +++ b/net/usr/bin/hostapdconfig.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python + +import configfile +import ip +import iw +import logging +import logging.config +from optparse import OptionParser + +logging.config.fileConfig("/etc/sololink.conf") +logger = logging.getLogger("net") + +logger.info("hostapdconfig.py starting") + +parser = OptionParser('hostapdconfig.py [options]') + +parser.add_option('--file', dest='fileName', type='string', + default='/etc/hostapd.conf', help='config file name') + +parser.add_option('--channel', dest='channel', type='string', + default=None, help='set channel') + +parser.add_option('--ssid', dest='ssid', type='string', + default=None, help='set ssid') + +parser.add_option('--ssidmac', dest='ssidmac', type='string', + default=None, help='append last 3 bytes of MAC to ssid') + +parser.add_option('--country', dest='country', type='string', + default=None, help='set the country code for regulatory') + +(opts, args) = parser.parse_args() + + +# Handle --channel - set channel in config file + +if opts.channel is not None: + logger.info('--channel %s', opts.channel) + + # integer: set specific channel (0 means auto) + # string: get interface's channel and use that + + # opts.channel might be string something like '9', so try to convert + # (can't use type()) + try: + chanInt = int(opts.channel) + except: + chanInt = None + + if chanInt is None: + # opts.channel should be an interface name, e.g. "wlan0" + ifName = opts.channel + freq = iw.getFreq(ifName) + if freq is None: + chanInt = 0 # same as acs_survey + logger.info('can\'t get %s frequency; defaulting to channel %d', ifName, chanInt) + else: + # Use wlan0's channel + chanInt = iw.freqToChan(freq) + logger.info('%s is at %d MHz (channel %d)', ifName, freq, chanInt) + + oldChan = configfile.paramGet(opts.fileName, 'channel') + logger.info('changing channel in %s from %s to %d', opts.fileName, str(oldChan), chanInt) + configfile.paramSet(opts.fileName, 'channel', chanInt) + + +# Handle --ssid - set SSID in config file + +if opts.ssid is not None: + logger.info('--ssid %s', opts.ssid) + oldSsid = configfile.paramGet(opts.fileName, 'ssid') + logger.info('changing ssid in %s from %s to %s', opts.fileName, oldSsid, opts.ssid) + configfile.paramSet(opts.fileName, 'ssid', opts.ssid) + + +# Handle --ssidmac - append last 3 bytes of MAC to ssid +# +# Note that common usage is supplying both --ssid and --ssidmac, e.g. +# hostapdconfig.py --ssid SoloLink_ --ssidmac wlan0 +# would set the ssid to something like SoloLink_505FC8 +# +# If the ssid already has the MAC appended, this will blindly append it again. + +if opts.ssidmac is not None: + logger.info('--ssidmac %s', opts.ssidmac) + mac = ip.getMac(opts.ssidmac) + if mac is None: + logger.error('can\'t get mac') + elif len(mac) != 17: + logger.error('mac "%s" is not 17 characters', mac) + else: + oldSsid = configfile.paramGet(opts.fileName, 'ssid') + mac = mac[9:11] + mac[12:14] + mac[15:17] + newSsid = oldSsid + mac.upper() + logger.info('changing ssid in %s from %s to %s', opts.fileName, oldSsid, newSsid) + configfile.paramSet(opts.fileName, 'ssid', newSsid) + +# Handle --country to set the country code +if opts.country is not None: + logger.info('--country %s', opts.country) + oldCountry = configfile.paramGet(opts.fileName, 'country_code') + logger.info('changing country code from %s to %s', oldCountry, opts.country) + configfile.paramSet(opts.fileName, 'country_code', opts.country) diff --git a/net/usr/bin/ip.py b/net/usr/bin/ip.py new file mode 100755 index 0000000..be93f51 --- /dev/null +++ b/net/usr/bin/ip.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +import re +import subprocess + + +# ip expected output: +# +# $ ip link +# 1: lo: mtu 65536 qdisc noqueue +# link/loopback 00:00:00:00:00:00 brd 00:00:00:00:00:00 +# 2: sit0: mtu 1480 qdisc noop +# link/sit 0.0.0.0 brd 0.0.0.0 +# 3: wlan0: mtu 1500 qdisc mq qlen 1000 +# link/ether 00:02:60:51:90:06 brd ff:ff:ff:ff:ff:ff + +def getMac(ifName): + try: + iwOut = subprocess.check_output(['ip', 'link'], stderr=subprocess.STDOUT) + except: + return None + m = re.search('^[0-9]+: ' + ifName + ': .*\n +link/[a-z]+ ([0-9a-f:]+) brd', iwOut, re.M) + if m: + return m.group(1) + return None + + +# Set the 02:00:00:00:00:00 bit in a MAC +# The purpose of this is to set the "locally administered" bit in a MAC +# (see http://en.wikipedia.org/wiki/MAC_address). The purpose of that is to +# generate a second MAC that we can use for simultaneous AP/STATION support +# for SoloLink. We handle the case where that bit is already set; it is not +# expected to be set, but if it is, we would rather clear it than fail. + +def macSetLocal(mac): + if len(mac) < 2: + return None + oldNibble = mac[1] + # 0->2, 1->3, 4->6, etc. + if oldNibble == '0': newNibble = '2' + elif oldNibble == '1': newNibble = '3' + elif oldNibble == '2': newNibble = '0' + elif oldNibble == '3': newNibble = '1' + elif oldNibble == '4': newNibble = '6' + elif oldNibble == '5': newNibble = '7' + elif oldNibble == '6': newNibble = '4' + elif oldNibble == '7': newNibble = '5' + elif oldNibble == '8': newNibble = 'a' + elif oldNibble == '9': newNibble = 'b' + elif oldNibble == 'a': newNibble = '8' + elif oldNibble == 'b': newNibble = '9' + elif oldNibble == 'c': newNibble = 'e' + elif oldNibble == 'd': newNibble = 'f' + elif oldNibble == 'e': newNibble = 'c' + elif oldNibble == 'f': newNibble = 'd' + else: + return None + return mac[0:1] + newNibble + mac[2:] + + +# Return last three bytes of MAC as a string +def mac3(ifname): + mac = getMac(ifname) + if len(mac) != 17: + return "" + return mac[9:11] + mac[12:14] + mac[15:17] + + +if __name__ == "__main__": + from optparse import OptionParser + parser = OptionParser('ip.py [options]') + parser.add_option('-3', '--mac3', dest='mac3_ifname', type='string', + default=None, + help='return last three bytes of interface\'s MAC') + (opts, args) = parser.parse_args() + if opts.mac3_ifname is not None: + print mac3(opts.mac3_ifname) diff --git a/net/usr/bin/iw.py b/net/usr/bin/iw.py new file mode 100644 index 0000000..464972c --- /dev/null +++ b/net/usr/bin/iw.py @@ -0,0 +1,119 @@ + +import re +import subprocess + + +# Expected output: +# +# $ iw dev wlan0 link +# Connected to d4:d7:48:81:68:a0 (on wlan0) +# SSID: 3DRobotics +# freq: 2447 +# RX: 5779050 bytes (38689 packets) +# TX: 13885 bytes (73 packets) +# signal: -37 dBm +# tx bitrate: 48.0 MBit/s +# +# bss flags: short-preamble short-slot-time +# dtim period: 0 +# beacon int: 85 +# Returns dictionary of information + +def link(ifName): + + info = { 'mac' : None, + 'ssid' : None, + 'freq' : None, + 'rxBytes' : None, + 'rxPackets' : None, + 'txBytes' : None, + 'txPackets' : None, + 'signal' : None, + 'txBitrate' : None } + + try: + iwOut = subprocess.check_output(['iw', 'dev', ifName, 'link'], + stderr=subprocess.STDOUT) + except: + return info + + m = re.search("Connected to (.*?)\s", iwOut) + if m: + info['mac'] = m.group(1) + + m = re.search('SSID: (.*)\s', iwOut) + if m: + info['ssid'] = m.group(1) + + m = re.search('freq: ([0-9]+)\s', iwOut) + if m: + info['freq'] = int(m.group(1)) + + m = re.search('RX: ([0-9]+) bytes \(([0-9]+) packets\)\s', iwOut) + if m: + info['rxBytes'] = int(m.group(1)) + info['rxPackets'] = int(m.group(2)) + + m = re.search('TX: ([0-9]+) bytes \(([0-9]+) packets\)\s', iwOut) + if m: + info['txBytes'] = int(m.group(1)) + info['txPackets'] = int(m.group(2)) + + m = re.search('signal: (-[0-9]+) dBm\s', iwOut) + if m: + info['signal'] = int(m.group(1)) + + m = re.search('tx bitrate: ([0-9]+\.[0-9]+) MBit', iwOut) + if m: + info['txBitrate'] = float(m.group(1)) + + return info + + +# returns frequency in MHz (or None) +def getFreq(ifName): + info = link(ifName) + return info['freq'] + + +# Expected output: +# +# $ iw dev wlan0 info +# Interface wlan0 +# ifindex 3 +# wdev 0x1 +# addr 00:02:60:51:90:06 +# ssid SoloLink +# type AP +# wiphy 0 +# channel 8 (2447 MHz), width: 20 MHz, center1: 2447 MHz + +def getChan(ifName): + try: + iwOut = subprocess.check_output(['iw', 'dev', ifName, 'info'], + stderr=subprocess.STDOUT) + except: + return None + m = re.search('channel ([0-9]+)\s', iwOut) + if m: + return int(m.group(1)) + return None + + +map = [(1, 2412), (2, 2417), (3, 2422), (4, 2427), (5, 2432), + (6, 2437), (7, 2442), (8, 2447), (9, 2452), (10, 2457), + (11, 2462), (12, 2467), (13, 2472), (14, 2484)] + + +def freqToChan(freq): + for m in map: + if freq == m[1]: + return m[0] + return None + + +def chanToFreq(chan): + for m in map: + if chan == m[0]: + return m[1] + return None diff --git a/net/usr/bin/rc_cli.py b/net/usr/bin/rc_cli.py new file mode 100755 index 0000000..d90913c --- /dev/null +++ b/net/usr/bin/rc_cli.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +import os +import socket +import threading +from sololink import rc_pkt + +def in_thread(sock): + while True: + pkt = sock.recv(1000) + if len(pkt) == rc_pkt.LENGTH: + # assume it's RC + timestamp, sequence, channels = rc_pkt.unpack(pkt) + pkt = "%d %d %d %d %d %d %d %d %d %d" % \ + (timestamp, sequence, + channels[0], channels[1], channels[2], channels[3], + channels[4], channels[5], channels[6], channels[7]) + print pkt + +sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) +sock.bind("/tmp/rc_cli." + str(os.getpid())) + +in_id = threading.Thread(target=in_thread, args=(sock,)) +in_id.daemon = True +in_id.start() + +while True: + s = raw_input() + sock.sendto(s, "/run/rc_uplink_cmd") diff --git a/net/usr/bin/rc_remap_sample.py b/net/usr/bin/rc_remap_sample.py new file mode 100755 index 0000000..646d63b --- /dev/null +++ b/net/usr/bin/rc_remap_sample.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python + +import os +import socket +import threading +import time +from sololink import rc_ipc +from sololink import rc_pkt + +# Sample RC remapper. This does the following: +# - detaches the uplink so it does not write packets to shared memory. +# - attaches itself to pixrc so it receives rc packets +# - for each received packet: +# - change the gimbal channel +# - write the packet to shared memory (so it goes to pixhawk) + +# If the RC uplink stops, the packets received by the remapper will contain +# the throttle failsafe value. Since throttle is passed through unchanged, +# pixhawk will failsafe. + +# If the remapper dies, nothing is updating the shared memory. The pixrc +# serial thread now detects this and injects a failsafe to pixhawk (sets +# throttle to zero). + + +# Read packets from pixrc. Replace the gimbal channel with values slewing +# up and down and output the packet to the shared memory. Sticks go through +# unchanged. +def remap_thread(sock): + gimbal = 1000 + up = True + while True: + s = sock.recv(1000) + if len(s) == rc_pkt.LENGTH: + # assume it's RC + timestamp, sequence, channels = rc_pkt.unpack(s) + if up: + gimbal += 10 + if gimbal >= 1520: + up = False + else: # down + gimbal -= 10 + if gimbal <= 1000: + up = True + #print channels[5], "->", gimbal + channels[5] = gimbal + if not rc_ipc.put((timestamp, sequence, channels)): + print "ERROR returned from rc_ipc.put" + # end if len(s)... + # end while True + + +sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) +sock.bind("/tmp/rc_remap." + str(os.getpid())) + +remap_id = threading.Thread(target=remap_thread, args=(sock,)) +remap_id.daemon = True +remap_id.start() + + +# Read from standard input and write whatever is received to the pixrc +# command socket. Intercept "start" and "stop" and send command sequences +# to the command socket starting and stopping the remapping. +while True: + s = raw_input() + if s == "START" or s == "start": + rc_ipc.attach() + # remapper thread is waiting for packets from the cmd socket but + # none come in because we have not done the "attach" yet + sock.sendto("detach uplink", "/run/rc_uplink_cmd") + # at this point nothing is writing to the packet shm + sock.sendto("attach", "/run/rc_uplink_cmd") + # now the remapper should be receiving packets on the command socket + # and for each one received, modifying it and writing it to the shm + elif s == "STOP" or s == "stop": + sock.sendto("detach", "/run/rc_uplink_cmd") + sock.sendto("attach uplink", "/run/rc_uplink_cmd") + # let the thread finish processing any queued rc packets + time.sleep(0.1) + # ...then detach the IPC + rc_ipc.detach() + else: + sock.sendto(s, "/run/rc_uplink_cmd") diff --git a/net/usr/bin/telem_ctrl.py b/net/usr/bin/telem_ctrl.py new file mode 100755 index 0000000..8b4a756 --- /dev/null +++ b/net/usr/bin/telem_ctrl.py @@ -0,0 +1,406 @@ +#!/usr/bin/env python + +# Forward telemetry to everyone attached to the AP. + +import ConfigParser +import datetime +import logging +import logging.config +import os +import re +import select +import socket +import sys +import time +import struct +import clock +from pymavlink import mavutil + +logging.config.fileConfig("/etc/sololink.conf") +logger = logging.getLogger("tlm") + +logger.info("starting") + +solo_conf = "/etc/sololink.conf" + +config = ConfigParser.SafeConfigParser() +config.read(solo_conf) + +hostapd_ctrl_sock_name = "/var/run/hostapd/wlan0-ap" + +# pairing module writes solo's IP address here when it is found +solo_address_file = "/var/run/solo.ip" + + +# read configuration items +try: + mav_dest_port = config.getint("solo", "mavDestPort") + telem_dest_port = config.getint("solo", "telemDestPort") + use_gps_time = config.getboolean("solo", "useGpsTime") +except: + logger.error("error reading config from %s", solo_conf) + sys.exit(1) + + + +def get_arp_table(): + """return ARP table as a list of tuples + + Only the MAC and IP are returned for each entry, since that is all that is + currently needed. Each entry is ("IP", "MAC"). + + IP address HW type Flags HW address Mask Device + 10.1.1.123 0x1 0x2 00:e0:4c:15:0a:df * wlan0-ap + 10.1.1.103 0x1 0x2 00:e0:4c:15:0a:df * wlan0-ap + 10.1.1.160 0x1 0x2 00:1f:09:04:00:24 * wlan0-ap + """ + arp_table = [] + + arp_table_file = "/proc/net/arp" + try: + f = open(arp_table_file) + except: + logger.error("can't open %s", arp_table_file) + return arp_table + + for line in f: + m = re.match("([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+).*?(\ +[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:\ +[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F])", line) + if m: + arp_table.append((m.group(1), m.group(2).lower())) + f.close() + + logger.debug("get_arp_table: %s", str(arp_table)) + + return arp_table + + + +def get_mac_ips(mac, arp_table=None): + """get MAC's IPs + + Given a MAC address, find it in the ARP table and return all IP addresses + for it. NOTE that one MAC can have multiple IPs. + """ + ips = [] + mac = mac.lower() + if arp_table is None: + arp_table = get_arp_table() + for entry in arp_table: + if entry[1] == mac: + ips.append(entry[0]) + return ips + + + +def get_stations(sock): + """get list of attached stations + + The return value is a list of tuples, where each tuple is ("mac", "ip"). + The MAC address will always be there, but the IP will only be there if + there is currently an entry in the ARP table. + """ + stations = [] + + arp_table = get_arp_table() + + # query hostapd for all attached stations + sock.sendto("STA-FIRST", hostapd_ctrl_sock_name) + p = sock.recv(1024) + while p: + lines = p.splitlines() + mac = lines[0] + ips = get_mac_ips(mac, arp_table) + for ip in ips: + stations.append((mac, ip)) + sock.sendto("STA-NEXT %s" % mac, hostapd_ctrl_sock_name) + p = sock.recv(1024) + + logger.debug("get_stations: %s", str(arp_table)) + + return stations + + +# wait for solo to attach, and get its IP address +while True: + try: + f = open(solo_address_file) + except IOError: + # no solo yet + time.sleep(1.0) + continue + data = f.read() + f.close() + solo_ip = data.strip() + break +### end while True + + +# hostapd control socket +hostapd_ctrl_sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + +# socket has to have a name even if we don't want to receive anything +hostapd_ctrl_local_name = "/tmp/telem_ctrl-%d" % os.getpid() +hostapd_ctrl_sock.bind(hostapd_ctrl_local_name) + +# port in solo to send telemetry to; this is set when we get the first +# downlink telemetry packet +solo_port = None + +# solo-side socket +# receives from solo, sends to solo +solo_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +solo_sock.bind(("", telem_dest_port)) + +# gcs-side socket +# receives from GCSes, sends to GCSes +gcs_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +gcs_sock.bind(("", 0)) + +read_socks = [solo_sock, gcs_sock] + +# index by tuple (src_system, src_component) +# corrupt packets in (-1, -1) +packets_down = { } +packets_down_drops = { } + +packets_down_total = 0 +packets_up_total = 0 + +# list of tuples ("mac", "ip") +current_stations = [] + +now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + +# how often to update list of stations to send telemetry to +station_update_interval_us = long(5 * 1000000) +station_update_time_us = now_us + +# how often to log packet counts +report_interval_us = long(10 * 1000000) +report_time_us = now_us + report_interval_us + +got_gps_time = False + +# last_down_sequence is indexed by a tuple (src_system, src_component) +# Each component has an independently running sequence, so we must keep track +# of each. +last_down_sequence = { } + +# Time when we can log another drop. Each time we log, we increment this by +# drop_log_min_us so we don't log more than that often. drop_log_count is the +# number of drops we did not log; it is printed to the log if it is not zero. +drop_log_us = 0 +drop_log_min_us = 1000000 # 1 second +drop_log_count = 0 + +# Time when we can log another corrupt packet. +drop_corrupt_us = 0 +drop_corrupt_min_us = 1000000 # 1 second +drop_corrupt_count = 0 + +while True: + + # check for packets to forward + ready = select.select(read_socks, [], [], 1.0) + + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + + if solo_sock in ready[0]: + # packet from solo + pkt, src_adrs = solo_sock.recvfrom(4096) + packets_down_total += 1 + + # check for "corrupt", where "corrupt" means we shouldn't even look + # at this packet + pkt_corrupt = False + pkt_len = len(pkt) + if pkt_len < 8: + pkt_corrupt = True + else: + # manual decode of mavlink header + magic = ord(pkt[0]) + if magic != 254: + pkt_corrupt = True + length = ord(pkt[1]) + if length != (pkt_len - 8): + pkt_corrupt = True + sequence = ord(pkt[2]) + src_sys = ord(pkt[3]) + src_comp = ord(pkt[4]) + msg_id = ord(pkt[5]) + + if pkt_corrupt: + + src = (-1, -1) + # Log corrupt packets, but don't flood the log + if src not in packets_down: + packets_down[src] = 1 # first one + else: + packets_down[src] += 1 # not first one + + if now_us >= drop_corrupt_us: + # loggit! + if drop_corrupt_count > 0: + logger.info("downlink: (skipped %d corrupt packets)", + drop_corrupt_count) + drop_corrupt_count = 0 + # pkt[:6] works even if pkt is less than 6 chars, e.g. + # '1234'[:6] = '1234' + logger.info("downlink: corrupt packet: %s", + str([ord(c) for c in pkt[:6]])) + drop_corrupt_us = now_us + drop_corrupt_min_us + else: + # don't log it + drop_corrupt_count += 1 + + else: # packet not corrupt + + src = (src_sys, src_comp) + + # initialize last_down_sequence[(system, component)] if necessary + # (happens for first message from a component) + if src not in last_down_sequence: + if sequence == 0: + last_down_sequence[src] = 255 + else: + last_down_sequence[src] = sequence - 1 + + expected_sequence = last_down_sequence[src] + 1 + if expected_sequence == 256: + expected_sequence = 0 + + # Log dropped packets, but don't flood the log. + # Ignore sequence numbers in gimbal messages (they are not right). + if (src_sys != 1 or src_comp != 154) and sequence != expected_sequence: + if src not in packets_down_drops: + packets_down_drops[src] = 0 + packets_down_drops[src] += 1 + if now_us >= drop_log_us: + # loggit! + logger.info("downlink: %d sequence errors: sys=%d comp=%d seq=%d", + 1 + drop_log_count, src_sys, src_comp, sequence) + drop_log_count = 0 + drop_log_us = now_us + drop_log_min_us + else: + # don't log it + drop_log_count += 1 + + last_down_sequence[src] = sequence + + # count packets from each source + if src not in packets_down: + packets_down[src] = 1 # first one + else: + packets_down[src] += 1 # not first one + + # The port on solo we send to varies; we save it from the first packet + # received. If we don't know the port yet and the packet is from solo, + # save the port number. + if src_adrs[0] == solo_ip: + if solo_port is None: + # first time we've see solo + logger.info("downlink: solo is at %s:%d", solo_ip, src_adrs[1]) + elif solo_port != src_adrs[1]: + # not first time we've see solo, but it's at a new port + logger.info("downlink: solo is now at %s:%d", solo_ip, src_adrs[1]) + solo_port = src_adrs[1] + + if not got_gps_time: + if msg_id == mavutil.mavlink.MAVLINK_MSG_ID_SYSTEM_TIME: + if len(pkt) == 20: # 6 + 12 + CRC + unix_usec, boot_msec = struct.unpack(" station_update_time_us: + + all_stations = get_stations(hostapd_ctrl_sock) + + # all_stations is list of tuples ("mac", "ip"). The MAC is always + # present, but the IP might be None if the ARP entry has timed out. + # A station is only added to current_stations if the IP is not None. + # A station is only removed from current_stations if the MAC is no + # longer there. (This code would be simpler if we knew that ARP + # entries never timed out.) + + # any new ones to add? + for new_station in all_stations: + if new_station[1] is None: + continue # no IP, can't add it even if new + if new_station[1] == solo_ip: + continue # don't send telemtry back to solo! + found = False + for old_station in current_stations: + if new_station[0] == old_station[0] and \ + new_station[1] == old_station[1]: # compare MACs and IPs + found = True + break + if not found: + logger.info("adding %s", str(new_station)) + current_stations.append(new_station) + + # any not there any more? + for old_station in current_stations: + found = False + for new_station in all_stations: + if old_station[0] == new_station[0]: + found = True + break + if not found: + logger.info("removing %s", str(old_station)) + current_stations.remove(old_station) + + station_update_time_us += station_update_interval_us + + ### end if now_us > station_update_time_us + + # time to report status? + if now_us > report_time_us: + msg = "downlink:" + for src in packets_down: + msg += " (%d,%d):%d" % (src[0], src[1], packets_down[src]) + packets_down = { } + if len(packets_down_drops) > 0: + msg += " -" + for src in packets_down_drops: + msg += " (%d,%d):%d" % (src[0], src[1], packets_down_drops[src]) + packets_down_drops = { } + msg += " uplink: %d" % packets_up_total + packets_up_total = 0 + logger.info(msg) + report_time_us += report_interval_us + +### end while True diff --git a/net/usr/bin/wpa_supplicant.py b/net/usr/bin/wpa_supplicant.py new file mode 100755 index 0000000..b6d491e --- /dev/null +++ b/net/usr/bin/wpa_supplicant.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python + +# Provide read/write access to wpa_supplicant.conf as a python dictionary + +# Comments are not preserved. +# A line starting with '#' as first non-whitespace is a comment. +# '#' after any non-whitespace is not special. +# Order is not preserved, except for the order of the network elements. +# The only ordered, compound-data element supported is the network list. +# No other elements may have { compound data }. +# Blobs are not supported. +# 'cred' is not supported. +# A network's opening { must be on the same line as the 'network' keyword. +# A network's closing } must be on a line by itself. +# +# A typical wpa_supplicant.conf dictionary might look as follows. Note that +# all keys are strings, and all values are lists of either strings or +# dictionaries. Storing the values as lists allows keeping them ordered; this +# is important for the network entries in the config file (order matters). +# Non-dictionary values are stored as lists simply because it keeps the +# structure more uniform, but also because there may be other config files +# this could work with where multiple entries are allowed. +# +# { +# 'ctrl_interface': ['/var/run/wpa_supplicant'], +# 'ctrl_interface_group': ['0'], +# 'manufacturer': ['3D Robotics'], +# 'model_name': ['Solo'], +# 'network': [ +# { +# 'auth_alg': ['OPEN'], +# 'key_mgmt': ['WPA-PSK'], +# 'pairwise': ['CCMP'], +# 'proto': ['RSN'], +# 'psk': ['"3drobotics2014"'], +# 'ssid': ['"SoloLink_Finley_Artoo"'] +# }, +# { +# 'auth_alg': ['OPEN'], +# 'key_mgmt': ['WPA-PSK'], +# 'pairwise': ['CCMP'], +# 'proto': ['RSN'], +# 'psk': ['"3drobotics2014"'], +# 'ssid': ['"SoloLink_Finley_Artoo"'] +# }, +# { +# 'auth_alg': ['OPEN'], +# 'key_mgmt': ['WPA-PSK'], +# 'pairwise': ['CCMP'], +# 'proto': ['RSN'], +# 'psk': ['"3drobotics2014"'], +# 'ssid': ['"SoloLink_Finley_Artoo"'] +# } +# ], +# 'update_config': ['1'] +# } +# +# The read() method populates this dictionary from a file. The write() method +# writes it to a file. Although the top-level order that items are written +# does not matter to wpa_supplicant, some ordering is allowed to make it a +# bit more maintainable. The list keyWriteOrder is a list of keys, defining +# the order they will be written to a file. When the dictionary is to be +# written, the code first goes through keyWriteOrder. For each entry therein, +# all entries from the dictionary with that key are written. After looping +# over keyWriteOrder, one more loop through the dictionary is done, and any +# keys found that are not in keyWriteOrder are written. + + +import re + + +# Internal: read lines from file, appending to supplied dictionary, until a +# closing brace is found or end of file is reached. If skip is True, then we +# are just reading until the closing brace or EOF is found, and ignoring the +# data. +def readDict(f, d, skip=False): + + while True: + + line = f.readline() + #print [ hex(ord(c)) for c in line ] + + if len(line) == 0: + # end of file, done + break + + # closing brace, possibly with leading or trailing whitespace, done + m = re.match('[\t ]*}[\t ]*$', line) + if m: + break + + if skip: + # ignore everything else + continue; + + # comment line, discard and continue + m = re.match('[\t ]*#', line) + if m: + continue + + # blank line, or whitespace only, discard and continue + m = re.match('[\t ]*$', line) + if m: + continue + + # Key is everything between the start of line and first '=', with + # whitespace trimmed from the start and end. Value is everything after + # the first '=' to the end of the line, also with whitespace trimmed + # from the start and end. Note that both key and value can have + # embedded whitespace, and the value can contain any character. There + # must be at least one '=' in the line; that is what currently breaks + # support for blobs. + m = re.match('[\t ]*(.+?)[\t ]*=[\t ]*(.+)$', line) + if m: + key = m.group(1) + val = m.group(2) + + if not key in d: + # initialize key as empty list + d[key] = [] + + if val != '{': + d[key].append(val) + else: + # starting a compound element, recurse + # can't read blobs (fix that if needed) + d2 = { } + d[key].append(d2) + m = re.match('blob-', key) + if m: + # don't try to parse blob data + readDict(f, d2, True) + else: + # read nested element as usual + readDict(f, d2) + else: + print 'Match fail:', [ hex(ord(c)) for c in line ] + + return d + + +# return dictionary with file contents +def read(fileName): + fd = open(fileName) # IOError + d = { } + readDict(fd, d) + return d + + +# When writing a dictionary, keys are written in this order. Any keys that are +# not in this list are written in an arbitrary order at the end. +keyWriteOrder = [ + 'ctrl_interface', + 'ctrl_interface_group', + 'update_config', + 'manufacturer', + 'model_name', + 'network', + # network element + 'ssid', + 'psk', + 'proto', + 'key_mgmt', + 'pairwise', + 'auth_alg' +] + + +# Internal: write one key to a file. +# Note that this recurses back into writeDict if the value is a dictionary. +def writeKey(fd, d, key, indent): + val = d[key] + # val is always a list. Each element in the list must be either a + # string or a dictionary. If it is a string, print a line of the form + # "key=val" to the file. If it is a dictionary, print a line of the + # form "key={", then print the dictionary, then print a line with only + # a closing brace "}". + for v in val: + if type(v) == str: + fd.write(indent + key + '=' + str(v) + '\n') + elif type(v) == dict: + fd.write(indent + key + '={\n') + writeDict(fd, v, indent + ' ') + fd.write(indent + '}\n') + else: + fd.write(indent + '# Unknown data:\n') + fd.write(indent + '# ' + key + '=' + str(val) + '\n') + + +# Internal: write dictionary to already-open file +def writeDict(fd, d, indent=''): + # Go through keys keyWriteOrder[], printing any that are in dictonary + for key in keyWriteOrder: + if key in d: + writeKey(fd, d, key, indent) + # Go through keys in dictionary, printing any not in keyWriteOrder + for key in d: + if key not in keyWriteOrder: + writeKey(fd, d, key, indent) + + +# write fileDict to fileName +def write(fileName, fileDict): + fd = open(fileName, 'w') # IOError + writeDict(fd, fileDict) + + +# Remove duplicate network entries from dictionary. +# Network entries are duplicates only if they are exactly the same. +def uniqueNetworks(d): + if not 'network' in d: + return + nets = d['network'] + # This top-level loop is done repeatedly until no duplicates are found. + # Each time a duplicate is found and deleted, we start over at the + # beginning, to avoid having to worry about what the indices mean after an + # item is deleted. + while True: + deleted = False + for i in range(len(nets)): + for j in range(len(nets)): + if j == i: + continue; + if nets[i] == nets[j]: + del(nets[j]) + deleted = True + break # for j + if deleted: + break # for i + # If nothing was deleted, we're done. If something was deleted, we + # start over at the beginning. + if not deleted: + break + + +if __name__ == '__main__': + import logging + import logging.config + from optparse import OptionParser + import sys + + logging.config.fileConfig("/etc/sololink.conf") + logger = logging.getLogger("net") + + logger.info("wpa_supplicant.py starting") + + parser = OptionParser('wpa_supplicant.py [options]') + + parser.add_option('-i', '--inFile', dest='inFile', type='string', + help='input file') + + parser.add_option('-o', '--outFile', dest='outFile', type='string', + help='output file') + + parser.add_option('-u', '--unique', dest='unique', action='store_true', + default=False, + help='remove duplicate network entries') + + (opts, args) = parser.parse_args() + + d = { } + + if opts.inFile is None: + fin = sys.stdin + logger.info("reading configuration from stdin") + else: + try: + fin = open(opts.inFile) + logger.info("reading configuration from \"%s\"", opts.inFile) + except: + logger.error("can't open \"%s\"", opts.inFile) + sys.exit(1) + + if opts.outFile is None: + fout = sys.stdout + logger.info("writing configuration to stdout") + else: + try: + fout = open(opts.outFile, 'w') + logger.info("writing configuration to \"%s\"", opts.outFile) + except: + logger.error("can't open \"%s\"", opts.outFile) + sys.exit(1) + + readDict(fin, d) + + if opts.unique: + uniqueNetworks(d) + + writeDict(fout, d) + + logger.info("done") diff --git a/pair/hostapd_ctrl.py b/pair/hostapd_ctrl.py new file mode 100644 index 0000000..7abb268 --- /dev/null +++ b/pair/hostapd_ctrl.py @@ -0,0 +1,113 @@ + +import os +import re +import socket +import time + +class HostapdCtrl: + + + def __init__(self, ifname): + # Arbitrary local socket name we bind to; this is needed because when + # we ATTACH to the hostapd control socket, it saves this name so it + # can send back events. + self._sockaddr_local = "/tmp/hostapd_ctrl.%d" % (os.getpid(), ) + try: + os.unlink(self._sockaddr_local) + except: + pass # it wasn't there, okay + self.sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self.sock.bind(self._sockaddr_local) + self.sock.setblocking(0) + # hostapd control socket. Connect to this to send commands to, and get + # responses and events from, hostapd. If the network interface used by + # hostapd changes, this must change. (It may be better to just pick + # the first socket in the directory.) + self._sockaddr_remote = "/var/run/hostapd/%s" % (ifname, ) + + + # Attach to hostapd control socket. + # + # When hostapd gets the ATTACH command, it saves the address of the socket + # that sent it (_sockaddr_local), then any asynchronous events that show + # up (like pin-needed) get sent there. There can be more than one client + # attached at the same time; hostapd keeps a list. + # + # Returns: + # True if attached + # False if not attached (timeout or error) + def attach(self, timeout_s): + self.sock.settimeout(0.1) + while True: + try: + self.sock.sendto("ATTACH", self._sockaddr_remote) + except: + # control socket probably not there + pass + else: + pkt = self.sock.recv(256) + # On success, we get "OK\n" (3 chars) + if pkt and len(pkt) == 3 and pkt.find("OK") != -1: + return True + timeout_s -= 1.0 + if timeout_s < 0.0: + return False + time.sleep(1.0) + # end while True + + + # Receive message from hostapd. + def recv(self): + return self.sock.recv(256) + + # Parse message from hostapd. + # + # The PIN request packet from hostapd looks something like this (with no + # newline; one space separates the two lines as shown here): + # + # <3>WPS-PIN-NEEDED 7d4c3eba-141a-54bc-9345-7bc0d6c27f04 00:15:6d:85:ce:b7 + # [My Solo|3D Robotics|Solo|Solo-1|S-1234567890|0-00000000-0] + # + # The UUID 7d4c3eba-141a-54bc-9345-7bc0d6c27f04 is generated by Solo. It + # is based on the MAC address somehow, but since we don't know how it is + # generated (or whether it will be the same over time), it is only used + # where necessary. + # + # The MAC 00:15:6d:85:ce:b7 comes from Solo's WiFi card. + # + # Everything in the [brackets] comes from Solo's wpa_supplicant.conf file: + # [device_name|manufacturer|model_name|model_number|serial_number|device_type] + # + # Returns: + # 9-tuple for WPS-PIN-NEEDED + # ("WPS-PIN-NEEDED", uuid, mac, device_name, manufacturer, + # model_name, model_number, serial_number, device_type) + # 1-tuple for other message types + # ("MESSAGE-TYPE", ) + # 1-tuple if message can't be parsed containing entire packet + def parse(self, pkt): + + regexp = "<3>WPS-PIN-NEEDED ([0-9a-fA-F\-]+) ([0-9a-fA-F:]+) \[(.*?)\|(.*?)\|(.*?)\|(.*?)\|(.*?)\|(.*?)\]" + m = re.match(regexp, pkt) + if m: + fields = m.groups() + if len(fields) == 8: + return ("WPS-PIN-NEEDED", ) + fields + else: + return (pkt, ) + + # add other messages here as needed + + # default: just parse out the message type + regexp = "<[0-9]+>(.+?) " + m = re.match(regexp, pkt) + if m: + return (m.group(1), ) + + return (pkt, ) + + + # Send pin reply + def send_pin(self, uuid, pin): + pin_reply = "WPS_PIN %s %d" % (uuid, pin) + self.sock.sendto(pin_reply, self._sockaddr_remote) diff --git a/pair/ifconfig.py b/pair/ifconfig.py new file mode 100644 index 0000000..c25d5a9 --- /dev/null +++ b/pair/ifconfig.py @@ -0,0 +1,36 @@ + +import re +import subprocess + + +def ip_mask(ifname): + """get IP address and netmask for an interface if it has them + + The return is a tuple containing the address and netmask as strings. If + either cannot be determined, None is returned in its place. If there is an + error running the ifconfig command, None is returned instead of the tuple. + + Example: + get_ip_mask("wlan0") + normally returns something like: + ("10.1.1.100", "255.255.255.0") + but could return something like: + ("10.1.1.100", None) + """ + + try: + out = subprocess.check_output(["ifconfig", ifname]) + except: + return None + + m = re.search("inet addr:([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+)", out) + if not m: + return None + ip = m.group(1) + + m = re.search("Mask:([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+)", out) + if not m: + return None + mask = m.group(1) + + return (ip, mask) diff --git a/pair/ip_util.py b/pair/ip_util.py new file mode 100644 index 0000000..8184aa7 --- /dev/null +++ b/pair/ip_util.py @@ -0,0 +1,39 @@ + +import os +import re + + +dhcp_lease_file = "/var/lib/misc/dnsmasq.leases" + + +def get_ip_mac(ip): + """get MAC from IP + + ARP table is in /proc/net/arp: + IP address HW type Flags HW address Mask Device + 10.1.1.101 0x1 0x2 e8:2a:ea:50:5f:c8 * wlan0-ap + 10.1.1.100 0x1 0x2 00:02:60:02:70:28 * wlan0-ap + + Returns None if IP is not in arp table. + """ + fn = "/proc/net/arp" + try: + f = open(fn) + except: + return None + for line in f: + m = re.match("([0-9]+\.[0-9]+\.[0-9]+\.[0-9]+).*?(\ +[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:\ +[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F]:[0-9a-fA-F][0-9a-fA-F])", line) + if m: + if m.group(1) == ip: + return m.group(2) + return None + + +def clear_dhcp_lease(client_mac): + new_lease_file = dhcp_lease_file + ".new" + os.system("grep -i -v %s %s > %s" % \ + (str(client_mac), dhcp_lease_file, new_lease_file)) + os.rename(new_lease_file, dhcp_lease_file) + os.system("/etc/init.d/dnsmasq restart") diff --git a/pair/pair.py b/pair/pair.py new file mode 100644 index 0000000..b89a988 --- /dev/null +++ b/pair/pair.py @@ -0,0 +1,51 @@ + +# Definitions shared between client and server connection modules + +# Messages/commands used in connection process + +# Network messages are all 68 bytes: +# start size description +# 0 1 CMD_* +# 1 1 SYS_* +# 2 1 cmd_data +# 3 1 locked +# 4 32 sololink version +# 36 32 firmware version (artoo or pixhawk) +# 68 message length + +# Messages/commands +CMD_CONN_REQ = 1 # client sends to request connection +CMD_CONN_ACK = 2 # Received to ack a connection request +CMD_USER_RSP = 3 # Internal; user response to connection request +CMD_TIMEOUT = 4 # Timeout waiting for next message + +# return name for command +def cmd_name(cmd): + if cmd == CMD_CONN_REQ: return "CMD_CONN_REQ" + elif cmd == CMD_CONN_ACK: return "CMD_CONN_ACK" + elif cmd == CMD_USER_RSP: return "CMD_USER_RSP" + elif cmd == CMD_TIMEOUT: return "CMD_TIMEOUT" + else: return "CMD_UNKNOWN" + +# System types that may use the connection protocol +SYS_CONTROLLER = 1 # +SYS_SOLO = 2 # + +# return name for system type +def sys_name(sys): + if sys == SYS_CONTROLLER: return "SYS_CONTROLLER" + elif sys == SYS_SOLO: return "SYS_SOLO" + elif sys == SYS_APP: return "SYS_APP" + else: return "SYS_UNKNOWN" + +# 'locked' byte in connect_request message +DATA_LOCKED = 1 # sender is locked + +# connected state moves from NO to PEND to YES +# network state moves from PEND to either NO or YES +NO = 1 +PEND = 2 +YES = 3 + +# all network messages are this long +CONN_MSG_LEN = 68 diff --git a/pair/pair_button.py b/pair/pair_button.py new file mode 100755 index 0000000..144dccb --- /dev/null +++ b/pair/pair_button.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python + +# Run this to simulate pressing the pair button on Solo. + +import struct +import sys + +button_filename = "/dev/input/event0" +button_error = False +button_file = None + +# event is: +# struct input_event { +# struct timeval time; +# unsigned short type; +# unsigned short code; +# unsigned int value; +# }; +# time: 8 bytes, not used here +# type: EV_SYN=0x0000, EV_KEY=0x0001 +# code: KEY_WPS_BUTTON=0x0211 +# value: 1 on push, 0 on release +# Fields are little endian. +# +# button push: +# xx xx xx xx xx xx xx xx 01 00 11 02 01 00 00 00 +# type=0x0001 code=0x0211 value=0x00000001 +# xx xx xx xx xx xx xx xx 00 00 00 00 00 00 00 00 +# type=0x0000 code=0x0000 value=0x00000000 +# +# button release: +# xx xx xx xx xx xx xx xx 01 00 11 02 00 00 00 00 +# type=0x0001 code=0x0211 value=0x00000000 +# xx xx xx xx xx xx xx xx 00 00 00 00 00 00 00 00 +# type=0x0000 code=0x0000 value=0x00000000 + +try: + f = open(button_filename, "w") +except: + print "can't open %s for writing" % button_filename + sys.exit(1) + +evt = struct.pack("@QHHi", 0, 0x0001, 0x0211, 1) +f.write(evt) + +evt = struct.pack("@QHHi", 0, 0, 0, 0) +f.write(evt) + +evt = struct.pack("@QHHi", 0, 0x0001, 0x0211, 0) +f.write(evt) + +evt = struct.pack("@QHHi", 0, 0, 0, 0) +f.write(evt) + +f.close() diff --git a/pair/pair_confirm.py b/pair/pair_confirm.py new file mode 100755 index 0000000..ae698a2 --- /dev/null +++ b/pair/pair_confirm.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python +import socket +import struct +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +confirm = struct.pack("!BBB", 3, 1, 1) +sock.sendto(confirm, ("127.0.0.1", 5501)) diff --git a/pair/pair_server.py b/pair/pair_server.py new file mode 100755 index 0000000..b034cea --- /dev/null +++ b/pair/pair_server.py @@ -0,0 +1,667 @@ +#!/usr/bin/env python + +# standard python +import ConfigParser +import logging +import logging.config +import re +import select +import socket +import struct +import sys +import time + +# sololink, in /usr/bin +import clock +import hostapd_ctrl +import ip_util +import lockout_msg +import pair +import runlevel + +# as printed to console and logs +prog_name = "pair_server" + +sololink_conf = "/etc/sololink.conf" +pairing_conf = "/log/3dr-pairing.conf" +sololink_version_file = "/VERSION" +firmware_version_file = "/STM_VERSION" +solo_sololink_version_file = "/tmp/PEER_SL_VERSION" +solo_firmware_version_file = "/tmp/PEER_FW_VERSION" + +logging.config.fileConfig(sololink_conf) +logger = logging.getLogger("pair") + +logger.info("%s starting", prog_name) + +sololink_config = ConfigParser.SafeConfigParser() +pairing_config = ConfigParser.SafeConfigParser() + +sololink_config.read(sololink_conf) +pairing_config.read(pairing_conf) + +# read configuration items +try: + # controller_link_port = 5501 + controller_link_port = \ + sololink_config.getint("pairing", "controller_link_port") + # solo_address_file = "/var/run/solo.ip" + solo_address_file = sololink_config.get("pairing", "solo_address_file") + # user_confirmation_timeout = 30.0 + user_confirmation_timeout = \ + sololink_config.getfloat("pairing", "user_confirmation_timeout") + # pair_req_port = 5013 + pair_req_port = sololink_config.getint("solo", "pairReqDestPort") + # pair_res_port = 5014 + pair_res_port = sololink_config.getint("solo", "pairResDestPort") +except: + logger.error("error reading config from %s", sololink_conf) + sys.exit(1) + +try: + check_versions = sololink_config.getboolean("solo", "pairCheckVersions") +except: + check_versions = True # default + logger.info("using default check_versions=%s", str(check_versions)) + +# read sololink version + +try: + f = open(sololink_version_file, 'r') + controller_sololink_version = f.readline() # still has \n + controller_sololink_version = controller_sololink_version.strip('\r\n\t\0 ') +except: + logger.error("error reading version from %s", sololink_version_file) + sys.exit(1) + +logger.info("sololink version \"%s\"", controller_sololink_version) + +# read firmware version + +try: + f = open(firmware_version_file, 'r') + controller_firmware_version = f.readline() # still has \n + controller_firmware_version = controller_firmware_version.strip('\r\n\t\0 ') +except: + logger.error("error reading version from %s", firmware_version_file) + controller_firmware_version = "unknown" + +logger.info("firmware version \"%s\"", controller_firmware_version) + +user_wait_timeout_us = int(user_confirmation_timeout * 1000000) +user_wait_start_us = None + +# It is not critical that the PIN be secret. The reason for using WPS PIN is +# not the PIN's security (it is not), but that we can cause hostapd to ask us +# for the PIN, at which time get confirmation from the user. +secret_pin = 74015887 + + +# States the controller side can be in +# STATE_IDLE: have not heard from a Solo yet +# STATE_USER_WAIT: have asked for user approval for a Solo +# STATE_CONNECTED: have connected to a Solo +# +# Events causing state transitions +# hostapd: pin request +# solo: connect request +# stm32: pair confirm +# timeout (waiting for user) +# +# Conditions affecting state transitions +# is message from the paired solo +# is message from the connected solo +# +# Actions to take on state transitions +# send pair request to stm32 +# send pair result to stm32 +# send pin response to hostapd +# send connect/yes to solo +# send connect/no to solo +# send connect/pend to solo +# set paired solo +# set connected solo +# +# A "paired solo" is a static thing, stored in a file. It is the Solo that we +# will accept a connection from without prompting the user. +# +# A "connected solo" is one that we have gotten a connect request from and +# sent a connect yes to. +# +# A solo becomes the "paired solo" by sending a pin request and having the +# user approve it. The "paired solo" is remembered in a file. +# +# A solo then becomes the "connected solo" by sending a connect request to +# which we respond with a connect yes. The "connected solo" is established +# some time after startup, and once established, cannot change until the +# controller is power cycled. +# +# At power on, there might be a paired solo, but there will not be a connected +# Solo. + + +STATE_IDLE = 0 +STATE_USER_WAIT = 1 +STATE_CONNECTED = 2 + +state_names = [ "IDLE", "USER_WAIT", "CONNECTED" ] + +def set_state(new_state): + global state + logger.info("%s -> %s", state_names[state], state_names[new_state]) + state = new_state + + +# information about the Solo we are currently talking to +class Solo: + # uuid: arrives from Solo with the PIN request, and is given back to + # hostapd with the PIN reply. Keeping it makes it so hostapd will give the + # password only to the Solo that the user approved by name (in case there + # happens to be more than one trying to pair at the same time). + # + # mac: used to identify a paired solo, i.e. once a user approves pairing + # with a particular MAC, future requests from the same MAC do not need + # approval. + # + # name: comes from wpa_supplicant.conf on Solo, and is what is presented + # to the user for approval. + def __init__(self): + self.reset() + def reset(self): + self.name = None + self.mac = None + self.uuid = None + self.need_pin = None + self.confirmed = None + self.locked = None + self.sololink_version = None + self.firmware_version = None + self.last_msg_us = None + def set(self, name, mac): + self.name = name + self.mac = mac + self.uuid = None + self.confirmed = None + self.locked = None + self.sololink_version = None + self.firmware_version = None + self.last_msg_us = None + def set_versions(self, sololink, firmware): + if self.sololink_version == sololink and self.firmware_version == firmware: + return + self.sololink_version = sololink + self.firmware_version = firmware + try: + f = open(solo_sololink_version_file, 'w') + f.write("%s\n" % (sololink, )) + f.close() + except: + logger.error("error writing solo sololink version to %s", solo_sololink_version_file) + try: + f = open(solo_firmware_version_file, 'w') + f.write("%s\n" % (firmware, )) + f.close() + except: + logger.error("error writing solo firmware version to %s", solo_firmware_version_file) + logger.info("solo version \"%s\"; firmware \"%s\"", + solo.sololink_version, solo.firmware_version) + + +solo = Solo() + + +# Set Solo's IP in /var/run/solo.ip, then change runlevels +def set_solo(solo_adrs): + logger.info("connected with %s", str(solo_adrs)) + # write solo address file + f = open(solo_address_file, "w") + f.write(solo_adrs[0] + "\n") + f.close() + + +# Timeouts all use CLOCK_MONOTONIC so as to not be disturbed by jumps +# in system time. Internal times all have either _s (seconds) or _us +# (microseconds) appended to the name to keep the units straight. Times +# from external sources (i.e. config settings) are all floating-point +# seconds. + + +# The stm32 module listens on different UDP ports, waiting for messages +# to send down to the STM32. +# pair request messages +stm32_req_sockaddr_remote = ("127.0.0.1", pair_req_port) +# pair result messages +stm32_res_sockaddr_remote = ("127.0.0.1", pair_res_port) + +# this server listens on this port; solo contacts this port to pair +pair_sockaddr_local = ("", controller_link_port) + + +# These come over in the pin-needed message, and so must match the settings in +# Solo's wpa_supplicant.conf file. Requiring these to match just lessens the +# liklihood that we get spurious pin-needed events from whoever happens to be +# out there. +manuf_name = "3D Robotics" +model_name = "Solo" + + +# Whether we have changed to runlevel.READY yet or not +runlevel_ready = False + + +# Log a version mismatch once per minute +version_mismatch_log_time_us = 0 +version_mismatch_log_interval_us = 60 * 1000000 + + +# If we are connected to a locked solo, but don't hear from it in this long, +# we take down the preflight update screen +solo_locked_msg_timeout_us = 3000000 # 3 sec + + +# Solo's that have been rejected since power on. Once a user rejects pairing +# for a Solo, that Solo should not cause a prompt again. The blacklist only +# lasts until the next reboot. It can't be persistent, in case a user +# accidently rejects his own Solo. +blacklist = [] + + +# Messages we might send to solo (part of the pairing protocol) +conn_ack_no = struct.pack(" 31: + name = name[:31] + name = name + "\0" + logger.debug("sending pair request to stm32 (%s)", name) + stm32_sock.sendto(name, stm32_req_sockaddr_remote) + +def send_pair_result(name): + if len(name) > 31: + name = name[:31] + name = name + "\0" + logger.debug("sending pair result to stm32 (%s)", name) + stm32_sock.sendto(name, stm32_res_sockaddr_remote) + + +def pin_req_valid(pin_req): + global solo + # msg is a 9-tuple + if len(pin_req) != 9: + solo.reset() + logger.info("pin request invalid") + logger.info(pin_req) + return False + uuid = pin_req[1] + mac = pin_req[2] + name = pin_req[3] + manufacturer = pin_req[4] + model_name = pin_req[5] + model_number = pin_req[6] + serial_number = pin_req[7] + device_type = pin_req[8] + # must be a 3DR Solo (allow model names like "Solo 1" or "Solo 2") + if manufacturer != "3D Robotics" or model_name.find("Solo") == -1: + solo.reset() + logger.info("pin request not from a 3DR Solo") + logger.info(msg) + return False + # must have a UUID in the request, since that's used to send back the PIN + if uuid is None: + solo.reset() + logger.info("pin request has no uuid") + logger.info(msg) + return False + # must have a MAC in the request, since that's used to differentiate Solos + if mac is None: + solo.reset() + logger.info("pin request has no mac") + logger.info(msg) + return False + # pin request is valid + logger.info("pin request: name=\"%s\", mac=%s, uuid=%s", + name, mac, uuid) + if name is None or name == "": + name = mac + solo.uuid = uuid + solo.mac = mac + solo.name = name + return True + + +def check_user_wait_timeout(now_us): + user_wait_us = now_us - user_wait_start_us; + if user_wait_us > user_wait_timeout_us: + # timeout! + logger.info("timeout waiting for user (%0.3f sec)", + user_wait_us / 1000000.0) + send_pair_result("") + solo.reset() + set_state(STATE_IDLE) + + +# Handle PIN request message from hostapd +# +# Only called when we are in STATE_IDLE and we got a WPS-PIN-NEEDED message +# from hostapd. +# +# Return next state: +# STATE_IDLE (pin request ignored) +# STATE_USER_WAIT (asked used to confirm) +# STATE_CONNECTED (known solo) +def handle_pin_request(now_us, msg): + global solo + global user_wait_start_us + logger.debug("handle_pin_request: %s", msg) + if not pin_req_valid(msg): + return STATE_IDLE + # pin_req_valid sets solo.uuid, solo.mac, and solo.name + # Has user rejected this solo since power on? + if solo.mac in blacklist: + logger.info("pin request from rejected solo; ignoring") + return STATE_IDLE + # Is this the paired Solo? (hmm, if so, it forgot its pairing) + if pairing_config.has_section(solo.mac): + logger.info("pin request from known solo") + # send pin reply to hostapd + hostapd.send_pin(solo.uuid, secret_pin) + logger.debug("sending pin for uuid %s", solo.uuid) + solo.confirmed = False + return STATE_CONNECTED + else: + logger.info("pin request from unknown solo; need user confirmation") + # send pair request to the STM32 + solo.need_pin = True + send_pair_request(solo.name) + user_wait_start_us = now_us + return STATE_USER_WAIT + + +# True if user accepted pairing +# False if user did not accepted pairing +def pair_confirm_answer(stm32_pkt): + return (ord(stm32_pkt[0]) != 0) + + +# validate connect request from solo +# True if packet valid +# False if packet not valid +def connect_request_valid(solo_pkt): + return (len(solo_pkt) == pair.CONN_MSG_LEN and \ + ord(solo_pkt[0]) == pair.CMD_CONN_REQ and \ + ord(solo_pkt[1]) == pair.SYS_SOLO and \ + ord(solo_pkt[2]) == 0) + # not checking solo_pkt[3] (locked flag) + # not checking solo_pkt[4:] (version) + + +def set_paired_solo(solo): + logger.info("setting %s as paired solo", solo.mac) + sections = pairing_config.sections() + for s in sections: + pairing_config.remove_section(s) + pairing_config.add_section(solo.mac) + pairing_config.set(solo.mac, "name", solo.name) + f = open(pairing_conf, "w") + pairing_config.write(f) + f.close() + + +def go_ready(): + global runlevel_ready + if not runlevel_ready: + logger.info("switching to runlevel.READY") + runlevel.set(runlevel.READY) + runlevel_ready = True + + +# stm32 interface socket. Send the pair request to the stm32 module using this +# socket, then get a pair confirm from the stm32 module. stm32_sock is not +# bound (gets INADDR_ANY and a random port). +stm32_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + +pair_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +pair_sock.bind(pair_sockaddr_local) + + +# hostapd control interface +hostapd = hostapd_ctrl.HostapdCtrl("wlan0-ap") + +if not hostapd.attach(2.0): + logger.error("can't attach to hostapd") + sys.exit(1) + + +# +# State transition loop +# + +state = STATE_IDLE + +while True: + + source_adrs = None + timeout = False + + # Could get message from: + # hostapd (pin request) + # solo (connect request) + # stm32 (pair confirm) + # ...or timeout waiting for a message + ready = select.select([hostapd.sock, stm32_sock, pair_sock], [], [], 1) + + # This is the only place the clock is read. If it is needed below (e.g. + # for user_wait_start_us), this value is used. Reading it again can lead + # to the condition where user_wait_start_us is temporarily later than + # now_us (it should always be earlier). + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + + if hostapd.sock in ready[0]: + # Can get several messages from hostapd, but we only respond to + # pin request + pkt = hostapd.sock.recv(256) + #logger.debug("%s: %s", prog_name, pkt) + msg = hostapd.parse(pkt) + # Ignore it we are not in STATE_IDLE or if it is not a pin request. + if state != STATE_IDLE: + logger.debug("ignoring message %s (not STATE_IDLE)", str(msg[0])) + elif msg[0] != "WPS-PIN-NEEDED": + logger.debug("ignoring message %s", str(msg[0])) + else: + set_state(handle_pin_request(now_us, msg)) + + elif stm32_sock in ready[0]: + # The only message from the STM32 we respond to is a pair_confirm, + # and we only respond to that if we are in STATE_USER_WAIT. + pkt = stm32_sock.recv(256) + logger.debug("message from stm32: %s", str([ord(c) for c in pkt])) + if state != STATE_USER_WAIT: + logger.debug("not STATE_USER_WAIT; ignoring message") + elif pair_confirm_answer(pkt): + logger.debug("user accepted pairing") + set_paired_solo(solo) + if solo.need_pin: + logger.debug("sending pin %d for uuid %s", + secret_pin, solo.uuid) + hostapd.send_pin(solo.uuid, secret_pin) + solo.confirmed = False + set_state(STATE_CONNECTED) + else: + logger.info("user rejected pairing") + send_pair_result("") + blacklist.append(solo.mac) + logger.info("blacklist: %s", str(blacklist)) + solo.reset() + set_state(STATE_IDLE) + + elif pair_sock in ready[0]: + # message from solo + pkt, source_adrs = pair_sock.recvfrom(256) + # the only valid message is a connect request + if not connect_request_valid(pkt): + logger.info("unknown message from %s", str(source_adrs)) + # ignore it + else: + source_mac = ip_util.get_ip_mac(source_adrs[0]) + solo_locked = ((ord(pkt[3]) & pair.DATA_LOCKED) != 0) + # source_mac might be None + if source_mac is not None: + logger.debug("connect request from %s", str(source_mac)) + if state == STATE_CONNECTED: + if source_mac == solo.mac: + logger.debug("send CONN_ACK (yes)") + pair_sock.sendto(conn_ack_yes, source_adrs) + solo.set_versions(pkt[4:36].strip('\r\n\t\0 '), + pkt[36:68].strip('\r\n\t\0 ')) + if not solo.confirmed: + send_pair_result(solo.name) + set_solo(source_adrs) + solo.confirmed = True + # look for changes in solo's lock state + if solo.locked is None: + # This happens when the user first confirms + # pairing; lock is not handled until after the + # pairing process (here). + if solo_locked: + # paired with a locked solo + logger.info("connected to locked solo") + lockout_msg.send_lock() + else: + logger.info("connected to unlocked solo") + # don't need to send an unlock message + elif not solo.locked: + # This is the usual case while flying; solo is not + # locked, and we'll find solo_locked (from the + # message) to be false + if solo_locked: + # solo was unlocked, but is now locked + # uncommon! + logger.info("solo is now locked") + lockout_msg.send_lock() + else: # solo.locked + if not solo_locked: + # solo was locked, and is now unlocked + # common when factory testing + logger.info("solo is now unlocked") + lockout_msg.send_unlock() + solo.locked = solo_locked + # check for version mismatch + if check_versions and (solo.sololink_version != controller_sololink_version): + # version mismatch - update required + # this really only needs to be sent once + lockout_msg.send_lock() + if now_us > version_mismatch_log_time_us: + logger.info("version mismatch: solo=\"%s\", controller=\"%s\"", + solo.sololink_version, controller_sololink_version) + version_mismatch_log_time_us = now_us + version_mismatch_log_interval_us + # XXX updating solo to matching version without + # restarting controller is probably not handled + # correctly + # change runlevel even if locked or versions incompatible + # apps look better if there is telemetry flowing and shotmgr is running + go_ready() + # remember the last time we heard from the connected solo + solo.last_msg_us = now_us + else: + # we have a connected solo, but the message is from a different solo + logger.info("send CONN_ACK (no)") + pair_sock.sendto(conn_ack_no, source_adrs) + elif state == STATE_IDLE: + # is this the paired Solo? + if pairing_config.has_section(source_mac): + logger.info("connection request from known solo: " + \ + "send CONN_ACK (yes)") + pair_sock.sendto(conn_ack_yes, source_adrs) + name = pairing_config.get(source_mac, "name") + solo.set(name, source_mac) + solo.confirmed = True + solo.locked = solo_locked + # strip whitespace and \0 + solo.set_versions(pkt[4:36].strip('\r\n\t\0 '), + pkt[36:68].strip('\r\n\t\0 ')) + set_state(STATE_CONNECTED) + set_solo(source_adrs) + if solo.locked: + # solo is locked - update required + lockout_msg.send_lock() + logger.info("connected to locked solo: solo=\"%s\", controller=\"%s\"", + solo.sololink_version, controller_sololink_version) + elif check_versions and (solo.sololink_version != controller_sololink_version): + # version mismatch - update required + lockout_msg.send_lock() + logger.info("version mismatch: solo=\"%s\", controller=\"%s\"", + solo.sololink_version, controller_sololink_version) + version_mismatch_log_time_us = now_us + version_mismatch_log_interval_us + # change runlevel even if locked or versions incompatible + # apps look better if there is telemetry flowing and shotmgr is running + go_ready() + # remember the last time we heard from the connected solo + solo.last_msg_us = now_us + # There is an edge case here, probably only stimulated + # by forcing it while testing: Artoo is entering its + # CONNECTED state. If one unpairs the Solo and reboots + # it, then Solo will come up and when the pair button is + # pressed, send a PIN request. This Artoo will not respond + # to it because it went straight from IDLE to CONNECTED. + # We cannot pre-load the PIN into hostapd at this point + # because we do not know the Solo's UUID. Artoo must be + # restarted to handle that case. + elif source_mac in blacklist: + # User has rejected this Solo since power on + logger.info("connection request from rejected solo: " + \ + "send CONN_ACK (no)") + pair_sock.sendto(conn_ack_no, source_adrs) + # stay in IDLE + else: + logger.info("connection request from unknown solo: " + \ + "send CONN_ACK (pending)") + pair_sock.sendto(conn_ack_pend, source_adrs) + # get from connect request message + # XXX generate it for now + name = "Solo " + source_mac[9:11] + \ + source_mac[12:14] + source_mac[15:17] + solo.set(name, source_mac) + solo.need_pin = False + send_pair_request(solo.name) + user_wait_start_us = now_us + # Don't check lockout and don't set solo.locked; need + # to allow pairing screens. Lockout is checked after + # pairing is confirmed. + set_state(STATE_USER_WAIT) + else: # STATE_USER_WAIT + if solo.mac == source_mac: + pair_sock.sendto(conn_ack_pend, source_adrs) + else: + pair_sock.sendto(conn_ack_no, source_adrs) + ### if source_mac is not None + + # all cases: check for timeout in STATE_USER_WAIT + if state == STATE_USER_WAIT: + check_user_wait_timeout(now_us) + + # All cases: check for timeout since we last heard from connected solo + # The purpose of this check is for the locked build case: If solo is + # turned off, we want to switch from "preflight update" to "waiting for + # solo". + if state == STATE_CONNECTED and \ + solo.locked and \ + (now_us - solo.last_msg_us) > solo_locked_msg_timeout_us: + # this causes the desired screen switch, but we stat connected + solo.locked = None + lockout_msg.send_unlock() + +# end while diff --git a/pair/pair_solo.py b/pair/pair_solo.py new file mode 100755 index 0000000..6b42de4 --- /dev/null +++ b/pair/pair_solo.py @@ -0,0 +1,578 @@ +#!/usr/bin/env python + +import errno +import ConfigParser +import logging +import logging.config +import os +import pair +import re +import select +import shutil +import socket +import struct +import sys +import time +sys.path.append("/usr/bin") +import clock +import ifconfig +import iw +import runlevel +import udhcpc +import wpa_control +import wpa_supplicant +import rc_lock + +sololink_conf = "/etc/sololink.conf" +wpa_supplicant_conf = "/etc/wpa_supplicant.conf" +wpa_supplicant_back = "/etc/wpa_supplicant.back" +sololink_version_file = "/VERSION" +firmware_version_file = "/PIX_VERSION" +controller_sololink_version_file = "/tmp/PEER_SL_VERSION" +controller_firmware_version_file = "/tmp/PEER_FW_VERSION" + +# defaults for items normally read from config file +controller_link_port = 5501 +wifi_connect_timeout = 5.0 +connect_request_interval = 1.0 +connect_ack_timeout = 0.5 +button_filename = "/dev/input/event0" +solo_ip = "10.1.1.10" +check_versions = True + +# It is not critical that the PIN be secret. The reason for using WPS PIN is +# not the PIN's security (it is not), but that we can cause hostapd to ask us +# for the PIN, at which time get confirmation from the user. +secret_pin = 74015887 + +button_error = False +button_file = None + +runlevel_ready = False + +# Log a version mismatch once per minute +version_mismatch_log_time_us = 0 +version_mismatch_log_interval_us = 60 * 1000000 + +controller_sololink_version = None +controller_firmware_version = None + +ifname = "wlan0" + +wpa = wpa_control.WpaControl(ifname) + + +# conditions: +# paired/not_paired: whether network exists in wpa_supplicant.conf + + +def pair_button(): + """get status of pairing button + + Returns True if pairing button has been pushed since the last call to this + function, or False otherwise. + """ + global button_error, button_file + if not button_error and button_file is None: + # open on first call + try: + button_file = open(button_filename) + except: + button_error = True + logger.error("can't open %s for reading", button_filename) + pushed = False + if not button_error: + # read all events, looking for pushes and ignoring others + while True: + r, w, x = select.select([button_file], [], [], 0) + if len(r) == 0: + # no more events + break + # button event + event = r[0].read(16) + if len(event) != 16: + logger.error("event not 16 bytes: len=%d, event=%s", + len(event), + str([hex(ord(x)) for x in event])) + # event is: + # struct input_event { + # struct timeval time; + # unsigned short type; + # unsigned short code; + # unsigned int value; + # }; + # time: 8 bytes, not used here + # type: EV_SYN=0x0000, EV_KEY=0x0001 + # code: KEY_WPS_BUTTON=0x0211 + # value: 1 on push, 0 on release + # Fields are little endian. + # + # button push: + # xx xx xx xx xx xx xx xx 01 00 11 02 01 00 00 00 + # type=0x0001 code=0x0211 value=0x00000001 + # xx xx xx xx xx xx xx xx 00 00 00 00 00 00 00 00 + # type=0x0000 code=0x0000 value=0x00000000 + # + # button release: + # xx xx xx xx xx xx xx xx 01 00 11 02 00 00 00 00 + # type=0x0001 code=0x0211 value=0x00000000 + # xx xx xx xx xx xx xx xx 00 00 00 00 00 00 00 00 + # type=0x0000 code=0x0000 value=0x00000000 + try: + s, t, c, v = struct.unpack("@QHHi", event) + if t == 0x0001 and c == 0x0211 and v == 1: + pushed = True + # keep reading events to flush out others + except: + logger.error("error unpacking input event: %s", + str([hex(ord(x)) for x in event])) + ### end while True + ### end if not button_error + return pushed + + +# Returns: +# First network name if there is at least one +# None if there are no networks +def wpa_supplicant_network_get(): + """get network from wpa_supplicant.conf + + Retrieve and return ssid from network={} section in wpa_supplicant.conf. + The first network found is returned. + """ + + try: + d = wpa_supplicant.read(wpa_supplicant_conf) + except: + logger.error("can't open %s for reading", wpa_supplicant_conf) + return None + + if "network" in d: + for net in d["network"]: + # net is a dictionary of net parameters + if "ssid" in net: + name = net["ssid"][0] + # strip leading and trailing quotes + if name[0] == "\"" and name[-1] == "\"": + name = name[1:-1] + return name + + # no network in wpa_supplicant.conf + return None + + +# Returns: +# True - Solo is paired +# False - Solo is not paired +def is_paired(): + # Solo is "paired" if there is a network in wpa_supplicant.conf + ssid = wpa_supplicant_network_get() + return (ssid is not None) + + +# Returns True or False +def pin_pair(): + logger.info("pin pair...") + wpa.pin_pair(secret_pin) + state = None + last_state = None + start_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + while True: + time.sleep(0.1) + stat = wpa.get_status() + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if "wpa_state" in stat: + state = stat["wpa_state"] + else: + state = None + if state != last_state: + if "ssid" in stat: + ssid = stat["ssid"] + else: + ssid = "" + if "bssid" in stat: + bssid = stat["bssid"] + else: + bssid = "" + logger.debug("%0.3f %s %s %s", (now_us - start_us) / 1000000.0, + state, ssid, bssid) + last_state = state + if state == "COMPLETED": + break + if state == "INACTIVE": + break + # end while + if state == "COMPLETED": + wpa.set("update_config", "1") + wpa.save() + wpa.set("update_config", "0") + os.system("md5sum %s > %s.md5" % \ + (wpa_supplicant_conf, wpa_supplicant_conf)) + os.system("sync") + logger.info("pin pair successful") + else: + logger.info("pin pair failed") + return (state == "COMPLETED") + + +# Returns: +# True associated +# False not associated, timeout +def associate(timeout): + logger.debug("associate") + wpa.reconfigure() + return wpa.network_connect(timeout) + + +def disassociate(): + logger.debug("disassociate") + wpa.network_disconnect() + + +# Returns: +# True got IP +# False error getting IP +def get_ip(): + logger.debug("get_ip") + udhcpc.start(ifname, hostname="solo") + ip_mask = ifconfig.ip_mask(ifname) + if ip_mask is None: + logger.info("pairing failed: error getting IP address") + udhcpc.stop() + return False + elif ip_mask[0] != solo_ip: + # This happens if this Solo and another Solo both know the wifi + # password, and: + # 1. The controller is paired to and already connected to the other + # Solo. This Solo will not get the correct IP unless the controller + # restarts. + # 2. The controller is paired to this Solo, but the other Solo got on + # the wifi network first and got the Solo IP. In that case, the + # other Solo will get booted off, and this Solo will get the + # correct IP when it retries. + logger.info("pairing failed: not at the fixed solo IP address") + udhcpc.stop() + return False + else: + logger.info("ip address %s netmask %s", ip_mask[0], ip_mask[1]) + return True + + +def release_ip(): + logger.debug("release_ip") + udhcpc.stop() + + +# Check network link status. If it is good (associated, have Solo IP address), +# return True. Otherwise, make sure everything is down (disassociated, udhcpc +# stopped, no IP) and return False. +# +# Returns: +# True associated, udhcpc running, at solo IP +# False not associated, udhcpc not running, no IP +def check_link(): + stat = wpa.get_status() + if ("wpa_state" in stat) and (stat["wpa_state"] == "COMPLETED"): + # wifi is associated + ip_mask = ifconfig.ip_mask(ifname) + if ip_mask and (ip_mask[0] == solo_ip): + return True + # something is not right; tear everything down + network_down() + return False + + +# Returns: +# True network is up +# False button was pushed (network is not up) +def network_up(): + logger.info("bringing up network...") + if check_link(): + logger.info("network already up") + return True + # check_link either confirms the network is up, or makes sure it is + # completely down + while True: + if associate(wifi_connect_timeout): + if get_ip(): + logger.info("network is up") + return True + disassociate() + # don't do network_remove_all here + if wait_button(2): + logger.info("network is down (button detected)") + return False + + +def network_down(): + release_ip() + disassociate() + wpa.network_remove_all() + + +# connected to controller, advance runlevel +def go_ready(): + global runlevel_ready + if not runlevel_ready: + logger.info("switching to runlevel.READY") + runlevel.set(runlevel.READY) + runlevel_ready = True + + +def set_controller_versions(pkt): + global controller_sololink_version + global controller_firmware_version + sololink = pkt[4:36].strip('\r\n\t\0 ') + firmware = pkt[36:68].strip('\r\n\t\0 ') + if controller_sololink_version == sololink and \ + controller_firmware_version == firmware: + return; + controller_sololink_version = sololink + controller_firmware_version = firmware + try: + f = open(controller_sololink_version_file, 'w') + f.write("%s\n" % (controller_sololink_version, )) + f.close() + except: + logger.error("error writing controller sololink version to %s", + controller_sololink_version_file) + try: + f = open(controller_firmware_version_file, 'w') + f.write("%s\n" % (controller_firmware_version, )) + f.close() + except: + logger.error("error writing controller firmware version to %s", + controller_firmware_version_file) + logger.info("controller version \"%s\"; firmware \"%s\"", + controller_sololink_version, controller_firmware_version) + + +# Returns +# Only if button pushed before any response from controller +def run_connected(): + global version_mismatch_log_time_us + ack_received = None + logger.info("establishing connection...") + controller_adrs = (controller_ip, controller_link_port) + confirmed = False + pending = False + pair_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + pair_sock.bind(("", 0)) # any port + pair_sock.settimeout(connect_ack_timeout) + send_error_logged = False + recv_error_logged = False + while True: + need_sleep = False + + if rc_lock.locked(): + locked = 1 + else: + locked = 0 + logger.debug("locked=%d", locked) + + conn_req = struct.pack(" version_mismatch_log_time_us: + logger.info("version mismatch: solo=\"%s\", controller=\"%s\"", + solo_sololink_version, controller_sololink_version) + version_mismatch_log_time_us = now_us + version_mismatch_log_interval_us + # Change runlevel even if locked or versions incompatible. + # Apps look better if there is telemetry flowing and shotmgr + # is running + go_ready() + elif ord(pkt[2]) == pair.PEND: + if not pending: + logger.info("connection pending") + pending = True + else: # pair.NO + if not confirmed: + # Controller says no. This Solo knows the wifi password + # from a previous pairing, but the controller has since + # been re-paired to a different Solo. + logger.info("connection rejected") + network_down() + return + else: + # Controller said yes to a previous connect request, but + # is now saying no. We are already in runlevel 4; + # something is really messed up. Ignore the nack. + logger.error("connection was up, now rejected") + else: + # mystery packet! + logger.error("mystery response received: %s", + str([ord(c) for c in pkt])) + time.sleep(connect_request_interval) + + +# Wait for pairing button +# +# Timeout = None means wait forever, else timeout in seconds +# +# Returns: +# True button pushed within timeout +# False timeout +def wait_button(timeout=None): + if timeout is None: + end_us = None + else: + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + \ + int(timeout * 1000000) + while True: + if pair_button(): + return True + if end_us is not None and \ + clock.gettime_us(clock.CLOCK_MONOTONIC) >= end_us: + return False + time.sleep(0.1) + + +def pair_solo(): + logger.info("pairing using %s", ifname) + # XXX update_config=0 should be default in wpa_supplicant.conf + wpa.set("update_config", "0") + while True: + if is_paired(): + if network_up(): + run_connected() + else: + network_down() + logger.info("waiting for pairing button") + wait_button() + pin_pair() + + +if __name__ == "__main__": + + logging.config.fileConfig(sololink_conf) + logger = logging.getLogger("pair") + + logger.info("pair_solo.py starting") + + config = ConfigParser.SafeConfigParser() + + # if the config file is not found, an empty list is returned and the "get" + # operations below fail + config.read(sololink_conf) + + # read configuration items + try: + controller_link_port = config.getint("pairing", "controller_link_port") + wifi_connect_timeout = \ + config.getfloat("pairing", "wifi_connect_timeout") + connect_request_interval = \ + config.getfloat("pairing", "connect_request_interval") + connect_ack_timeout = config.getfloat("pairing", "connect_ack_timeout") + button_filename = config.get("pairing", "button_filename") + solo_ip = config.get("solo", "soloIp") + controller_ip = config.get("solo", "artooIp") + except: + logger.error("error reading config from %s", sololink_conf) + sys.exit(1) + + try: + check_versions = sololink_config.getboolean("solo", "pairCheckVersions") + except: + check_versions = True # default + logger.info("using default check_versions=%s", str(check_versions)) + + # read sololink version + try: + f = open(sololink_version_file, 'r') + solo_sololink_version = f.readline() # still has \n + solo_sololink_version = solo_sololink_version.strip('\r\n\t\0 ') + except: + logger.error("error reading version from %s", sololink_version_file) + sys.exit(1) + + logger.info("sololink version \"%s\"", solo_sololink_version) + + # read firmware version + try: + f = open(firmware_version_file, 'r') + solo_firmware_version = f.readline() # still has \n + solo_firmware_version = solo_firmware_version.strip('\r\n\t\0 ') + except: + logger.error("error reading version from %s", firmware_version_file) + solo_firmware_version = "unknown" + + logger.info("firmware version \"%s\"", solo_firmware_version) + + # If /etc/.rc_lock exists, delete it (SOLO-709) + if os.path.isfile("/etc/.rc_lock"): + logger.info("deleting /etc/.rc_lock") + os.unlink("/etc/.rc_lock") + + pair_solo() + # pair_solo never returns diff --git a/pair/runlevel.py b/pair/runlevel.py new file mode 100644 index 0000000..a49527d --- /dev/null +++ b/pair/runlevel.py @@ -0,0 +1,14 @@ + +import subprocess + +STANDBY = 3 +READY = 4 +MAINTENANCE = 5 + +def set(level): + subprocess.check_output(["init", str(level)]) + +def get(): + r = subprocess.check_output(["runlevel"]) + r = r.split() + return int(r[1]) diff --git a/pair/udhcpc.py b/pair/udhcpc.py new file mode 100644 index 0000000..bed3b0f --- /dev/null +++ b/pair/udhcpc.py @@ -0,0 +1,79 @@ + +import os +import subprocess + +# udhcpc is normally started from busybox's ifup, with compiled-in parameters: +# udhcpc -R -n -p /var/run/udhcpc.wlan0.pid -i wlan0 +# If wlan0 does not associate on boot, udhcpc tries to get a lease, fails, and +# exits (because of the -n parameter). +# +# Busybox udhcpc: +# +# udhcpc [-Cfbnqtvo] [-c CID] [-V VCLS] [-H HOSTNAME] [-i INTERFACE] +# [-p pidfile] [-r IP] [-s script] [-O dhcp-option] ... +# +# -V,--vendorclass=CLASSID Vendor class identifier +# -i,--interface=INTERFACE Interface to use (default eth0) +# -H,-h,--hostname=HOSTNAME Client hostname +# -c,--clientid=CLIENTID Client identifier +# -C,--clientid-none Suppress default client identifier +# -p,--pidfile=file Create pidfile +# -r,--request=IP IP address to request +# -s,--script=file Run file at DHCP events +# (default /usr/share/udhcpc/default.script) +# -t,--retries=N Send up to N request packets +# -T,--timeout=N Try to get a lease for N seconds (default 3) +# -A,--tryagain=N Wait N seconds (default 20) after failure +# -O,--request-option=OPT Request DHCP option OPT (cumulative) +# -o,--no-default-options Do not request any options +# (unless -O is also given) +# -f,--foreground Run in foreground +# -b,--background Background if lease is not immediately obtained +# -S,--syslog Log to syslog too +# -n,--now Exit with failure if lease is not immediately +# obtained +# -q,--quit Quit after obtaining lease +# -R,--release Release IP on quit +# -a,--arping Use arping to validate offered address + + +def start(ifname, hostname=None, retries=3): + cmd = ["udhcpc", "-R", "-n", + "-p", "/var/run/udhcpc." + ifname + ".pid", + "-i", ifname] + if hostname is not None: + cmd.extend(["-x", "hostname:%s" % hostname]) + cmd.extend(["-T", "1", + "-t", str(retries)]) + try: + subprocess.check_output(cmd) + except: + pass + + +def stop(): + try: + # stderr is redirected for this one to avoid error messages when + # there is no udhcpc process running + subprocess.check_output(["killall", "udhcpc"], + stderr=subprocess.STDOUT) + except: + pass + + +def pid(ifname): + try: + f = open("/var/run/udhcpc." + ifname + ".pid") + pid = int(f.read()) + f.close() + return pid + except: + return None + + +def get_lease(pid): + try: + subprocess.check_output(["kill", "-USR1", pid]) + return True + except: + return False diff --git a/pair/wpa_cli.py b/pair/wpa_cli.py new file mode 100644 index 0000000..ba7cc06 --- /dev/null +++ b/pair/wpa_cli.py @@ -0,0 +1,139 @@ + +import clock +import re +import subprocess + +def run_cmd(ifname, cmd): + """run a wpa_cli command, return the output""" + cmd.insert(0, "wpa_cli") + cmd.insert(1, "-i" + ifname) + return subprocess.check_output(cmd) + +def run_cmd_ok(ifname, cmd): + """run a wpa_cli command that should return OK""" + cmd.insert(0, "wpa_cli") + cmd.insert(1, "-i" + ifname) + out = subprocess.check_output(cmd) + m = re.match("OK", out) + if not m: + raise RuntimeError + +def run_cmd_int(ifname, cmd): + """run a wpa_cli command that should return an integer""" + cmd.insert(0, "wpa_cli") + cmd.insert(1, "-i" + ifname) + out = subprocess.check_output(cmd) + m = re.match("([0-9]+)", out) + if not m: + raise RuntimeError + # m.group(1) is still a string, e.g "2" + return m.group(1) + +def get_status(ifname): + """get status from wpa_cli""" + cmd = ["wpa_cli", "-i" + ifname, "status"] + out = subprocess.check_output(cmd) + out = out.splitlines() + status = {} + for line in out: + m = re.match("(.+?)=(.+)", line) + if not m: + raise RuntimeError + status[m.group(1)] = m.group(2) + return status + +# wpa_status polling, observed states: +# (times shown are one particular connection and may vary quite a bit) +# +# Initially: +# wpa_state=DISCONNECTED +# address=00:02:60:02:70:28 +# +# After ~25 msec: +# wpa_state=SCANNING +# address=00:02:60:02:70:28 +# +# After ~700 msec: +# wpa_state=ASSOCIATING +# address=00:02:60:02:70:28 +# +# After ~750 msec: +# bssid=02:02:60:02:70:25 +# ssid=SoloLink_090909 +# id=0 +# mode=station +# pairwise_cipher=NONE +# group_cipher=NONE +# key_mgmt=NONE +# wpa_state=COMPLETED +# address=00:02:60:02:70:28 +def poll_status(ifname, final_state, timeout=None, verbose=False): + """poll status until it reaches a given state or timeout""" + if timeout is not None: + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + (timeout * 1000000) + else: + end_us = None + last_stat = {} + while True: + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + stat = get_status(ifname) + if verbose and stat != last_stat: + print stat + last_stat = stat + if "wpa_state" in stat: + new_state = stat["wpa_state"] + else: + new_state = "" + if new_state == final_state: + return True + if end_us is not None and now_us > end_us: + return False + +def network_add(ifname, ssid): + net_num = run_cmd_int(ifname, ["add_network"]) + run_cmd_ok(ifname, ["select_network", net_num]) + run_cmd_ok(ifname, ["enable_network", net_num]) + run_cmd_ok(ifname, ["set_network", net_num, "ssid", "\"" + ssid + "\""]) + run_cmd_ok(ifname, ["set_network", net_num, "key_mgmt", "NONE"]) + return net_num + +def network_connect(ifname, timeout): + run_cmd_ok(ifname, ["reassociate"]) + return poll_status(ifname, "COMPLETED", timeout) + +def network_disconnect(ifname): + run_cmd_ok(ifname, ["disconnect"]) + +def network_remove(ifname, net_num): + run_cmd_ok(ifname, ["remove_network", net_num]) + +def network_remove_all(ifname): + for net_num in range(10): + run_cmd(ifname, ["remove_network", str(net_num)]) + +def save(ifname): + run_cmd_ok(ifname, ["save_config"]) + +def set(ifname, variable, value): + run_cmd_ok(ifname, ["set", variable, value]) + +def pin_pair(ifname, pin): + return run_cmd(ifname, ["wps_pin", "any", str(pin)]) + +def reconfigure(ifname): + return run_cmd(ifname, ["reconfigure"]) + +import datetime + +def wps_logged(): + log = open("/log/wps.log", "a") + pin_pair("wlan0", 74015887) + last_stat = {} + while True: + stat = get_status("wlan0") + if stat != last_stat: + log.write(str(datetime.datetime.now())) + log.write(": ") + log.write(str(stat)) + log.write("\n") + last_stat = stat diff --git a/pair/wpa_control.py b/pair/wpa_control.py new file mode 100644 index 0000000..42ca04c --- /dev/null +++ b/pair/wpa_control.py @@ -0,0 +1,158 @@ + +import clock +import os +import socket +import time + + +class WpaControl: + + def __init__(self, ifname, verbose=False): + # Arbitrary local socket name we bind to; this is needed because when + # we send a command to the wpa_supplicant control socket, it sends the + # reply back whence the request came. + self.sockaddr_local = "/tmp/wpa_ctrl.%d" % (os.getpid(), ) + try: + os.unlink(self.sockaddr_local) + except: + pass # it wasn't there, okay + self.sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self.sock.bind(self.sockaddr_local) + # wpa_supplicant control socket. Connect to this to send commands to + # and get responses from wpa_supplicant. If the network interface used + # by wpa_supplicant changes, this must change. (It may be better to + # just pick the first socket in the directory.) + self.sockaddr_remote = "/var/run/wpa_supplicant/%s" % (ifname, ) + self.verbose = verbose + + def run_cmd(self, cmd): + """run a command and return the output""" + if self.verbose: + start_us = clock.gettime_us(CLOCK_MONOTONIC) + try: + self.sock.sendto(cmd, self.sockaddr_remote) + except: + # control socket probably gone + rsp = "" + else: + rsp = self.sock.recv(1024) + if self.verbose: + end_us = clock.gettime_us(CLOCK_MONOTONIC) + print "command \"%s\", response \"%s\" in %0.3f sec" % \ + (cmd, rsp, (end_us - start_us) / 1000000.0) + return rsp + + def run_cmd_ok(self, cmd): + """run a command that should return OK or FAIL""" + rsp = self.run_cmd(cmd) + rsp = rsp.strip() + if rsp == "OK": + return True + if rsp == "FAIL": + return False + print "run_cmd_ok: rsp=\"%s\"" % (rsp, ) + raise RuntimeError + + def run_cmd_int(self, cmd): + """run a command that should return an integer""" + rsp = self.run_cmd(cmd) + rsp = rsp.strip() + # will either convert okay or raise exception + return int(rsp) + + def get_status(self): + """get status""" + rsp = self.run_cmd("STATUS") + rsp = rsp.splitlines() + status = {} + for line in rsp: + line = line.strip() + m = line.split("=", 1) + if len(m) != 2: + raise RuntimeError + status[m[0]] = m[1] + return status + + # wpa_status polling, observed states: + # (times shown are one particular connection and may vary quite a bit) + # + # Initially: + # wpa_state=DISCONNECTED + # address=00:02:60:02:70:28 + # + # After ~25 msec: + # wpa_state=SCANNING + # address=00:02:60:02:70:28 + # + # After ~700 msec: + # wpa_state=ASSOCIATING + # address=00:02:60:02:70:28 + # + # After ~750 msec: + # bssid=02:02:60:02:70:25 + # ssid=SoloLink_090909 + # id=0 + # mode=station + # pairwise_cipher=NONE + # group_cipher=NONE + # key_mgmt=NONE + # wpa_state=COMPLETED + # address=00:02:60:02:70:28 + def poll_status(self, final_state, timeout=None, poll_delay=0.001): + """poll status until it reaches a given state or timeout""" + if timeout is not None: + end_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + (timeout * 1000000) + else: + end_us = None + last_status = None + verbose_orig = self.verbose + self.verbose = False + while True: + status = self.get_status() + now_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + if verbose_orig and status != last_status: + print status + last_status = status + if "wpa_state" in status and status["wpa_state"] == final_state: + self.verbose = verbose_orig + return True + if end_us is not None and now_us > end_us: + self.verbose = verbose_orig + return False + # To get accurate timings (with verbose=True), this timeout should + # be small (or zero). That leads to high CPU usage, so the default + time.sleep(poll_delay) + + def network_add(self, ssid): + net_num = self.run_cmd_int("ADD_NETWORK") + self.run_cmd_ok("SELECT_NETWORK %d" % (net_num, )) + self.run_cmd_ok("ENABLE_NETWORK %d" % (net_num, )) + self.run_cmd_ok("SET_NETWORK %d ssid \"%s\"" % (net_num, ssid)) + self.run_cmd_ok("SET_NETWORK %d key_mgmt NONE" % (net_num, )) + return net_num + + def network_connect(self, timeout): + self.run_cmd_ok("REASSOCIATE") + return self.poll_status("COMPLETED", timeout, poll_delay=0.1) + + def network_disconnect(self): + self.run_cmd_ok("DISCONNECT") + + def network_remove(self, net_num): + self.run_cmd_ok("REMOVE_NETWORK %d" % (net_num, )) + + def network_remove_all(self): + for net_num in range(10): + self.network_remove(net_num) + + def save(self): + self.run_cmd_ok("SAVE_CONFIG") + + def set(self, variable, value): + self.run_cmd_ok("SET %s %s" % (variable, str(value))) + + def pin_pair(self, pin): + return self.run_cmd("WPS_PIN any %s" % (str(pin), )) + + def reconfigure(self): + return self.run_cmd("RECONFIGURE") diff --git a/pair/wpa_supplicant_init b/pair/wpa_supplicant_init new file mode 100755 index 0000000..1fdf174 --- /dev/null +++ b/pair/wpa_supplicant_init @@ -0,0 +1,19 @@ +#!/bin/sh + +# wpa_supplicant one-time initialization +# +# If Solo's name in wpa_supplicant.conf is the default, +# change it to something unique. + +WPA_SUPPLICANT_CONF=/etc/wpa_supplicant.conf + +# Return true if Solo's name in wpa_supplicant.conf is still the default +soloNameIsDefault() { + grep -i -q "^device_name=Solo Default" $1 +} + +if soloNameIsDefault ${WPA_SUPPLICANT_CONF}; then + soloname="Solo `/usr/bin/ip.py --mac3 wlan0`" + sed -i "s/^device_name=Solo Default/device_name=${soloname}/" ${WPA_SUPPLICANT_CONF} + md5sum ${WPA_SUPPLICANT_CONF} > ${WPA_SUPPLICANT_CONF}.md5 +fi diff --git a/pair/wps_confirm.py b/pair/wps_confirm.py new file mode 100755 index 0000000..bd57c9f --- /dev/null +++ b/pair/wps_confirm.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python + +# This does the equivalent of: +# hostapd_cli wps_pin any + +import socket + +print "wps_confirm.py starting" + +controlSock = "/var/run/hostapd/wlan0-ap" + +# Message sent to hostapd to confirm PIN pairing. The PIN embedded here must +# match the PIN in Solo's script wps_request.py. It must be a valid WPS PIN; +# use: +# hostapd_cli wps_check_pin +# wpa_cli wps_check_pin +# to validate a pin, or: +# wpa_cli wps_pin get +# to get a new random pin. +pinMessage = "WPS_PIN any 74015887" + +s = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + +try: + s.sendto(pinMessage, controlSock) + print "pin confirm sent to %s" % (controlSock, ) +except: + print "ERROR sending pin confirm to %s" % (controlSock, ) diff --git a/pair/wps_pair_msg.py b/pair/wps_pair_msg.py new file mode 100644 index 0000000..83c9c5b --- /dev/null +++ b/pair/wps_pair_msg.py @@ -0,0 +1,54 @@ + +import struct + +""" +Pair Request/Confirm message sent between pairing and stm32 modules. + +Start +Byte Size Description +0 4 ID (not used) +4 4 Length following (56) +8 4 Flags +12 20 MAC address, ASCII string (e.g. "02:1F:09:03:00:08") +32 32 Friendly name (e.g. "Solo Mynis") +64 (total packet length) +""" + +MAC_LENGTH = 20 +NAME_LENGTH = 32 +BODY_LENGTH = 56 +TOTAL_LENGTH = 64 + +FLAGS_REQUEST = 1 +FLAGS_CONFIRM = 2 +FLAGS_RESULT = 3 + +fmt_string = " "Hel" +# struct.pack("8s", "Hello") -> "Hello\0\0\0" +# +# struct.unpack will return a string of exactly the specified length, e.g. +# struct.unpack("8s", "Hello\0\0\0") -> "Hello\0\0\0" + +def unpack(s): + if len(s) != TOTAL_LENGTH: + return None + hdr_id, hdr_len, mac, name, flags = struct.unpack(fmt_string, s) + mac = mac.strip("\0") + name = name.strip("\0") + return mac, name, flags + +def pack(mac, name, flags=0): + # truncate MAC and Name if necessary so packed strings are \0 terminated + if len(mac) > (MAC_LENGTH - 1): + mac = mac[:(MAC_LENGTH - 1)] + if len(name) > (NAME_LENGTH - 1): + name = name[:(NAME_LENGTH - 1)] + return struct.pack(fmt_string, MSG_ID, BODY_LENGTH, mac, name, flags) diff --git a/px_uploader/loadPixhawk.py b/px_uploader/loadPixhawk.py new file mode 100755 index 0000000..a22e3a1 --- /dev/null +++ b/px_uploader/loadPixhawk.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python + +import subprocess +import sys +import os +import time +from pymavlink import mavutil +import glob +import ConfigParser +import shutil +from datetime import datetime +import re +import json +import argparse + +SELECT_GPIO = "21" +ENABLE_GPIO = "19" + +#GPIO direction set +def setGPIODir(gpio, direction): + dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w") + dir_fd.write(direction) + dir_fd.close() + +#Open the GPIO +def openGPIO(gpio): + #Check and see if the GPIO is already exported + if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)): + #otherwise export it + exp_fd = open("/sys/class/gpio/export", "w") + exp_fd.write(gpio) + exp_fd.close() + + setGPIODir(gpio, "out"); + +def closeGPIO(gpio): + unexp_fd = open("/sys/class/gpio/unexport", "w") + unexp_fd.write(gpio) + unexp_fd.close() + +def setGPIO(gpio, value): + val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w") + val_fd.write(value) + val_fd.close() + +def openSetClose(gpio, value): + openGPIO(gpio) + setGPIO(gpio, value) + closeGPIO(gpio) + +def writeVerFile(ArduVersion, PX4Version, NuttXVersion, filename): + verFile = open("/PIX_VERSION","w") + version=filename.split('-',1)[1].split('.px',1)[0] + verFile.write(version + '\n') + verFile.write(ArduVersion + '\n') + verFile.write(PX4Version + '\n') + verFile.write(NuttXVersion + '\n') + verFile.close() + +def disconnectAndExit(): + openSetClose(SELECT_GPIO, "0") + openSetClose(ENABLE_GPIO, "1") + os.system("echo none > /sys/class/leds/user2/trigger") + sys.exit() + +#Bootloading process +print "Pixhawk bootloader" + +parser = argparse.ArgumentParser() +parser.add_argument("file_specified", nargs='?') +args = parser.parse_args() + +if args.file_specified and not os.path.isfile(args.file_specified): + print "File \"%s\" not found" % (args.file_specified) + sys.exit() +if args.file_specified: + latest = args.file_specified +else: + #See what version we have + files = glob.glob('/firmware/*.px4') + if not files: + print "No Pixhawk firmware available for update." + sys.exit() + + latest = files[-1] + +#Get the versions from px4 json +latest_json = json.loads(open(latest).read()) +try: + ArduVersion = latest_json['ardupilot_git_hash'][:8] +except: + print ".px4 file does not have ardupilot git hash" + ArduVersion = "00000000" + PX4Version = "00000000" + NuttXVersion = "00000000" +else: + PX4Version = latest_json['px4_git_hash'][:8] + NuttXVersion = latest_json['nuttx_git_hash'][:8] + +print "Read file versions:" +print " ArduPilot: " + ArduVersion +print " PX4: " + PX4Version +print " NuttX: " + NuttXVersion + +#Set the LED to a blink pattern so the user knows we're updating +os.system("echo timer > /sys/class/leds/user2/trigger") +os.system("echo 1000 > /sys/class/leds/user2/delay_on") +os.system("echo 100 > /sys/class/leds/user2/delay_off") + +#Set the USB select GPIOs +openSetClose(SELECT_GPIO, "1") +openSetClose(ENABLE_GPIO, "0") + +time.sleep(1) + +usb_devs = glob.glob('/dev/serial/by-id/usb-3D*') +if not usb_devs: + print "No pixhawk found on USB. Exiting." + disconnectAndExit() + +print "Pixhawk found on USB." +pixhawk_usb = usb_devs[-1] +m = mavutil.mavlink_connection(pixhawk_usb) + +#Check the arducopter version number from the AUTOPILOT_VERSION message +if not args.file_specified: + m.mav.autopilot_version_request_send(m.target_system, m.target_component) + while True: + msg = m.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=5) + if msg: + pixArduVersion = ''.join(chr(e) for e in msg.flight_custom_version) + pixPX4Version = ''.join(chr(e) for e in msg.middleware_custom_version) + pixNuttXVersion = ''.join(chr(e) for e in msg.os_custom_version) + + print "Pixhawk returned versions:" + print " ArduPilot: " + pixArduVersion + print " PX4: " + pixPX4Version + print " NuttX: " + pixNuttXVersion + + if(ArduVersion != pixArduVersion or PX4Version != pixPX4Version or NuttXVersion != pixNuttXVersion): + print "New version available, bootloading" + break + else: + print "Already latest version" + m.close() + try: + os.mkdir("/firmware/loaded/") + except OSError: + pass + os.rename(latest,"/firmware/loaded/"+os.path.basename(latest)) + writeVerFile(ArduVersion,PX4Version,NuttXVersion,latest) + disconnectAndExit() + else: + print "Unable to get Ardupilot version, forcing bootload." + break + +m.close() + +print "loading file %s" % (os.path.abspath(latest)) + +#Bootload the pixhawk now (probably not the best way to do this) +ret = subprocess.call(["python","/usr/bin/px_uploader.py","--port=/dev/serial/by-id/usb-3D_Robotics*",latest]) + +if(ret != 0): + print "Unable to bootload the pixhawk, closing" +else: + print "Succesfully bootloaded Pixhawk" + #move the loaded firmware to a new /firmware/loaded/ folder + shutil.rmtree("/firmware/loaded/", ignore_errors=True) + try: + os.mkdir("/firmware/loaded/") + except OSError: + pass + + if args.file_specified: + shutil.copy(latest,"/firmware/loaded/"+os.path.basename(latest)) + else: + os.rename(latest,"/firmware/loaded/"+os.path.basename(latest)); + writeVerFile(ArduVersion,PX4Version,NuttXVersion,latest) + +disconnectAndExit() diff --git a/px_uploader/px_uploader.py b/px_uploader/px_uploader.py new file mode 100755 index 0000000..3cdd9d1 --- /dev/null +++ b/px_uploader/px_uploader.py @@ -0,0 +1,613 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Serial firmware uploader for the PX4FMU bootloader +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# +# The uploader uses the following fields from the firmware file: +# +# image +# The firmware that will be uploaded. +# image_size +# The size of the firmware in bytes. +# board_id +# The board for which the firmware is intended. +# board_revision +# Currently only used for informational purposes. +# + +# for python2.7 compatibility +from __future__ import print_function + +import sys +import argparse +import binascii +import serial +import struct +import json +import zlib +import base64 +import time +import array +import os + +from sys import platform as _platform + + +class firmware(object): + '''Loads a firmware file''' + + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) + crcpad = bytearray(b'\xff\xff\xff\xff') + + def __init__(self, path): + + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() + + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') + + def property(self, propname): + return self.desc[propname] + + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state + + +class uploader(object): + '''Uploads a firmware file to the PX FMU bootloader''' + + # protocol bytes + INSYNC = b'\x12' + EOC = b'\x20' + + # reply bytes + OK = b'\x10' + FAILED = b'\x11' + INVALID = b'\x13' # rev3+ + + # command bytes + NOP = b'\x00' # guaranteed to be discarded by the bootloader + GET_SYNC = b'\x21' + GET_DEVICE = b'\x22' + CHIP_ERASE = b'\x23' + CHIP_VERIFY = b'\x24' # rev2 only + PROG_MULTI = b'\x27' + READ_MULTI = b'\x28' # rev2 only + GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area + GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay + REBOOT = b'\x30' + + INFO_BL_REV = b'\x01' # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 4 # maximum supported bootloader protocol + INFO_BOARD_ID = b'\x02' # board type + INFO_BOARD_REV = b'\x03' # board revision + INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes + + PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 252 # protocol max is 255 + + NSH_INIT = bytearray(b'\x0d\x0d\x0d') + NSH_REBOOT_BL = b"reboot -b\n" + NSH_REBOOT = b"reboot\n" + MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') + MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') + + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' + + def close(self): + if self.port is not None: + self.port.close() + + def __send(self, c): +# print("send " + binascii.hexlify(c)) + self.port.write(c) + + def __recv(self, count=1): + c = self.port.read(count) + if len(c) < 1: + raise RuntimeError("timeout waiting for data (%u bytes)" % count) +# print("recv " + binascii.hexlify(c)) + return c + + def __recv_int(self): + raw = self.__recv(4) + val = struct.unpack("= 9.0: + self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0) + else: + self.__drawProgressBar(label, 10.0, 10.0) + sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) + sys.stdout.flush() + + if self.__trySync(): + self.__drawProgressBar(label, 10.0, 10.0) + return; + + raise RuntimeError("timed out waiting for erase") + + # send a PROG_MULTI command to write a collection of bytes + def __program_multi(self, data): + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.PROG_MULTI) + self.__send(length) + self.__send(data) + self.__send(uploader.EOC) + self.__getSync() + + # verify multiple bytes in flash + def __verify_multi(self, data): + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.READ_MULTI) + self.__send(length) + self.__send(uploader.EOC) + self.port.flush() + programmed = self.__recv(len(data)) + if programmed != data: + print("got " + binascii.hexlify(programmed)) + print("expect " + binascii.hexlify(data)) + return False + self.__getSync() + return True + + # send the reboot command + def __reboot(self): + self.__send(uploader.REBOOT + + uploader.EOC) + self.port.flush() + + # v3+ can report failure if the first word flash fails + if self.bl_rev >= 3: + self.__getSync() + + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] + + # upload code + def __program(self, label, fw): + print("\n", end='') + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + + uploadProgress = 0 + for bytes in groups: + self.__program_multi(bytes) + + #Print upload progress (throttled, so it does not delay upload progress) + uploadProgress += 1 + if uploadProgress % 256 == 0: + self.__drawProgressBar(label, uploadProgress, len(groups)) + self.__drawProgressBar(label, 100, 100) + + # verify code + def __verify_v2(self, label, fw): + print("\n", end='') + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + verifyProgress = 0 + for bytes in groups: + verifyProgress += 1 + if verifyProgress % 256 == 0: + self.__drawProgressBar(label, verifyProgress, len(groups)) + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") + self.__drawProgressBar(label, 100, 100) + + def __verify_v3(self, label, fw): + print("\n", end='') + self.__drawProgressBar(label, 1, 100) + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + verifyProgress = 0 + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") + self.__drawProgressBar(label, 100, 100) + + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() + + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") + + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( + self.board_type, fw.property('board_id')) + if args.force: + print("WARNING: %s" % msg) + else: + raise IOError(msg) + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('Latin-1') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + try: + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user + print('') + print("chip: %08x" % self.__getCHIP()) + except Exception: + # ignore bad character encodings + pass + + self.__erase("Erase ") + self.__program("Program", fw) + + if self.bl_rev == 2: + self.__verify_v2("Verify ", fw) + else: + self.__verify_v3("Verify ", fw) + + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + + print("\nRebooting.\n") + self.__reboot() + self.port.close() + + def send_reboot(self): + try: + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + except: + return + + +# Detect python version +if sys.version_info[0] < 3: + runningPython3 = False +else: + runningPython3 = True + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") +parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') +parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") +args = parser.parse_args() + +# warn people about ModemManager which interferes badly with Pixhawk +if os.path.exists("/usr/sbin/ModemManager"): + print("==========================================================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") + print("==========================================================================================================") + +# Load the firmware file +fw = firmware(args.firmware) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") + +# Spin waiting for a device to show up +try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + + except Exception: + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") + up.send_reboot() + + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.5) + + # always close the port + up.close() + continue + + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + # print the error + print("\nERROR: %s" % ex.args) + + except IOError as e: + up.close(); + continue + + finally: + # always close the port + up.close() + + # we could loop here if we wanted to wait for more boards... + sys.exit(0) + + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) + +# CTRL+C aborts the upload/spin-lock by interrupt mechanics +except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0) diff --git a/tools/3dcp b/tools/3dcp new file mode 100755 index 0000000..0d84bf8 --- /dev/null +++ b/tools/3dcp @@ -0,0 +1,22 @@ +#!/bin/bash +# Helper script for scp'ing onto solo and artoo +# +# Syntax: 3dcp [ artoo | solo ] localfile remotepath +# Note: Use absolute paths for sololink location +# +# Example usage: +# 3dcp artoo test.py /home/root +# 3dcp solo update.tar.gz /log/updates +################################################## + +CUR_SCRIPT_DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +USERNAME="root" +TARGET="10.1.1.1" + +if [ "$1" == "solo" ] +then + TARGET="10.1.1.10" +fi + +scp -i $CUR_SCRIPT_DIR/updater/updater_id_rsa $2 $USERNAME@$TARGET:$3 diff --git a/tools/3dsh b/tools/3dsh new file mode 100755 index 0000000..596f393 --- /dev/null +++ b/tools/3dsh @@ -0,0 +1,18 @@ +#!/bin/bash +# Helper script for ssh'ing into solo and artoo +# +# Syntax: 3dsh [ solo | artoo ] +# Note: Defaults to artoo without an argument +############################################### + +CUR_SCRIPT_DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +USERNAME="root" +TARGET="10.1.1.1" + +if [ "$1" == "solo" ] +then + TARGET="10.1.1.10" +fi + +ssh -i $CUR_SCRIPT_DIR/updater/updater_id_rsa $USERNAME@$TARGET diff --git a/tools/build/clang-format-run.py b/tools/build/clang-format-run.py new file mode 100755 index 0000000..711165e --- /dev/null +++ b/tools/build/clang-format-run.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +import os, sys, subprocess + +extensions = ( + ".cpp", ".cxx", ".c++", ".cc", ".cp", + ".c",".i", ".ii", ".h", ".h++", ".hpp", + ".hxx", ".hh", ".inl", ".inc", ".ipp", + ".ixx", ".txx", ".tpp", ".tcc", ".tpl" +) + +def find_clang_format(options): + for c in options: + try: + v = subprocess.check_output([c, "--version"]) + return c, v.strip() + except: + pass + print "can't find clang-format in %s" % str(clang_format_list) + sys.exit(1) + +# find the installed version of clang-format - we require at least 3.6 +clang_format_list = ("clang-format-3.6", "clang-format-3.7", "clang-format") +clang_format, cf_version = find_clang_format(clang_format_list) + +# report differences unless "--apply" is given as argument +do_apply = (len(sys.argv) >= 2) and (sys.argv[1] == "--apply") + +diff_files = [] + +for root, dirs, files in os.walk("."): + if 'build' in dirs: + dirs.remove('build') + if 'install' in dirs: + dirs.remove('install') + for file in files: + if file.endswith(extensions): + fpath = os.path.join(root, file) + if do_apply: + subprocess.check_call([clang_format, "-i", "-style=file", fpath], stdout=subprocess.PIPE) + else: + # compare the output of clang-format with the current file, complain if there's a diff + p1 = subprocess.Popen([clang_format, "-style=file", fpath], stdout=subprocess.PIPE) + p2 = subprocess.Popen(["diff", "-u", fpath, "-"], stdin=p1.stdout, stdout=subprocess.PIPE) + if p2.wait() != 0: + diff_files.append(fpath) + +if len(diff_files) != 0: + print cf_version, "reported differences for the following files:" + for f in diff_files: + print " ", f + sys.exit(1) diff --git a/tools/build/sololink_new b/tools/build/sololink_new new file mode 100755 index 0000000..d4d1a84 --- /dev/null +++ b/tools/build/sololink_new @@ -0,0 +1,82 @@ +#!/bin/bash + +# Uncomment one of these +#machineList="imx6solo_3dr_artoo imx6solo_3dr imx6solo_3dr_analogtvin imx6solo_3dr_1080p" +machineList="imx6solo_3dr_artoo imx6solo_3dr_1080p" + +if [ -z "${1}" ]; then + bspBranch=imx-3.10.17-1.0.0_ga + dateStr=`date +%Y%m%d_%H%M` + buildName=sololink_$dateStr +else + bspBranch=${1} + buildName=${1} + #buildName=sololink_v${1} +fi + +buildRoot=. + +mkdir $buildRoot/$buildName +pushd $buildRoot/$buildName + +set -x + +( + +echo "`date`: ${buildName}: start" + +repo init -u git@github.com:3drobotics/3dr-arm-yocto-bsp.git -b ${bspBranch} +repo sync + +export EULA=1 +source ./setup-environment build + +if true; then + +# increase number of threads +cpu_count=`grep -c ^processor /proc/cpuinfo` +new_count=$(($cpu_count * 4)) +echo "setting to ${new_count} threads" +sed -i "s/^BB_NUMBER_THREADS = '[0-9]\+'/BB_NUMBER_THREADS = '${new_count}'/" conf/local.conf +sed -i "s/^PARALLEL_MAKE = '-j [0-9]\+'/PARALLEL_MAKE = '-j ${new_count}'/" conf/local.conf + +fi + +if false; then + +# use local sources +cat >> conf/local.conf <&1 | tee console.log + +set +x +popd diff --git a/tools/logdownload/grabSoloLinkLogs.sh b/tools/logdownload/grabSoloLinkLogs.sh new file mode 100755 index 0000000..95fe519 --- /dev/null +++ b/tools/logdownload/grabSoloLinkLogs.sh @@ -0,0 +1,14 @@ +#!/bin/sh + +SCPARGS="-o StrictHostKeyChecking=no -i ../updater/updater_id_rsa" + +DATE=`date "+%Y-%m-%d-%H:%M:%S"` + +mkdir sololink-$DATE +mkdir artoolink-$DATE + +ssh-keygen -R 10.1.1.1 &> /dev/null +ssh-keygen -R 10.1.1.10 &> /dev/null + +scp $SCPARGS -r root@10.1.1.1:/log/* ./artoolink-$DATE/ +scp $SCPARGS -r root@10.1.1.10:/log/* ./sololink-$DATE/ diff --git a/tools/scripts/px_usb_switch.py b/tools/scripts/px_usb_switch.py new file mode 100644 index 0000000..c318dbd --- /dev/null +++ b/tools/scripts/px_usb_switch.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python + +import sys +import os +import time +import argparse + +SELECT_GPIO = "21" +ENABLE_GPIO = "19" + +#GPIO direction set +def setGPIODir(gpio, direction): + dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w") + dir_fd.write(direction) + dir_fd.close() + +#Open the GPIO +def openGPIO(gpio): + #Check and see if the GPIO is already exported + if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)): + #otherwise export it + exp_fd = open("/sys/class/gpio/export", "w") + exp_fd.write(gpio) + exp_fd.close() + + setGPIODir(gpio, "out"); + +def closeGPIO(gpio): + unexp_fd = open("/sys/class/gpio/unexport", "w") + unexp_fd.write(gpio) + unexp_fd.close() + +def setGPIO(gpio, value): + val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w") + val_fd.write(value) + val_fd.close() + +def openSetClose(gpio, value): + openGPIO(gpio) + setGPIO(gpio, value) + closeGPIO(gpio) + +parser = argparse.ArgumentParser() +parser.add_argument("state", help='enable [1] or disable [0] Pixhawk USB') +args = parser.parse_args() + +if args.state == '1': + print("Enabling PH2 USB") + openSetClose(SELECT_GPIO, "1") + openSetClose(ENABLE_GPIO, "0") +else: + print("Disabling PH2 USB") + openSetClose(SELECT_GPIO, "0") + openSetClose(ENABLE_GPIO, "1") diff --git a/tools/setregdomain/setCountryCode.sh b/tools/setregdomain/setCountryCode.sh new file mode 100755 index 0000000..b11ae91 --- /dev/null +++ b/tools/setregdomain/setCountryCode.sh @@ -0,0 +1,90 @@ +#!/bin/bash + +SCPARGS="-o StrictHostKeyChecking=no -i ../updater/updater_id_rsa" +SSHCMD="ssh $SCPARGS root@10.1.1.1" +GOLDPARTDEV="/dev/mmcblk0p1" + +#Sets the regulatory domain of the Artoo to the input argument. +#Note the country code must be a 2-letter all-caps code + +COUNTRY_LIST='AD,AE,AF,AI,AL,AM,AN,AR,AT,AU,AW,AZ,BA,BB,BD,BE,BF,BG,BH,BL,BN,BO,BR,BT,BY,BZ,CA,CF,CH,CI,CL,CN,CO,CR,CY,CZ,DE,DK,DM,DO,DZ,EC,EE,EG,ES,FI,FM,FR,GE,GB,GD,GH,GR,GL,GT,GU,GY,HN,HK,HR,HT,HU,ID,IE,IL,IN,IS,IR,IT,JM,JP,JO,KE,KH,KN,KP,KR,KW,KZ,LB,LC,LI,LK,LS,LT,LU,LV,MC,MA,MD,ME,MF,MO,MH,MK,MR,MT,MY,MX,NL,NO,NP,NZ,OM,PA,PE,PG,PH,PK,PL,PM,PT,PR,PW,QA,RO,RS,RU,RW,SA,SE,SG,SI,SK,SN,SV,SY,TC,TD,TG,TW,TH,TT,TN,TR,UA,US,UY,UZ,VC,VE,VN,VU,WF,YE,ZA,ZW' + +usage() { +echo " +Usage ./setCountryCode.sh CODE + +where CODE is a two-letter country code in the following list: +-------------------------------------------------------------- +AD,AE,AF,AI,AL,AM,AN,AR,AT,AU,AW,AZ,BA,BB,BD,BE,BF,BG,BH,BL,BN, +BO,BR,BT,BY,BZ,CA,CF,CH,CI,CL,CN,CO,CR,CY,CZ,DE,DK,DM,DO,DZ,EC, +EE,EG,ES,FI,FM,FR,GE,GB,GD,GH,GR,GL,GT,GU,GY,HN,HK,HR,HT,HU,ID, +IE,IL,IN,IS,IR,IT,JM,JP,JO,KE,KH,KN,KP,KR,KW,KZ,LB,LC,LI,LK,LS, +LT,LU,LV,MC,MA,MD,ME,MF,MO,MH,MK,MR,MT,MY,MX,NL,NO,NP,NZ,OM,PA, +PE,PG,PH,PK,PL,PM,PT,PR,PW,QA,RO,RS,RU,RW,SA,SE,SG,SI,SK,SN,SV, +SY,TC,TD,TG,TW,TH,TT,TN,TR,UA,US,UY,UZ,VC,VE,VN,VU,WF,YE,ZA,ZW" +exit +} + +if [ -z "$1" ]; then usage; fi +if [[ ! $1 =~ ^[A-Z]{2}$ ]]; then usage; fi +if [[ "${COUNTRY_LIST}" =~ "$1" ]]; then + echo "Setting to country $1" +else + usage +fi +COUNTRY=$1 + +ssh-keygen -R 10.1.1.1 &> /dev/null + +#See if we're booted from the GOLDEN partiton +#if so, remount it rw. Otherwise mount the GOLDEN partition and set the .reg file. +BOOTPART=`ssh $SCPARGS root@10.1.1.1 grep 'boot' /proc/mounts | awk '{print $1}'` +if [ $BOOTPART == $GOLDPARTDEV ]; then + + echo "operating from GOLDEN partition." + + echo "remounting GOLDEN partition read-write" + ssh $SCPARGS root@10.1.1.1 "mount $GOLDPARTDEV /mnt/boot -o remount,rw" + + echo "removing any existing reg file" + ssh $SCPARGS root@10.1.1.1 "rm -f /mnt/boot/.reg" + + echo "creating reg file" + ssh $SCPARGS root@10.1.1.1 "echo $COUNTRY >> /mnt/boot/.reg" + + echo "syncing" + ssh $SCPARGS root@10.1.1.1 "sync" + + echo "remounting boot partition read-only" + ssh $SCPARGS root@10.1.1.1 "mount $GOLDPARTDEV /mnt/boot -o remount,ro" + + echo "done" + +else + + echo "operating from LATEST partition" + + ssh $SCPARGS root@10.1.1.1 "mkdir -p /tmp/bootmnt" + + echo "mounting GOLDEN partition read-write" + ssh $SCPARGS root@10.1.1.1 "mount $GOLDPARTDEV /tmp/bootmnt" + + echo "removing any existing reg file" + ssh $SCPARGS root@10.1.1.1 "rm -f /tmp/bootmnt/.reg" + + echo "creating reg file" + ssh $SCPARGS root@10.1.1.1 "echo $COUNTRY >> /tmp/bootmnt/.reg" + + echo "syncing" + ssh $SCPARGS root@10.1.1.1 "sync" + + echo "unmounting GOLDEN partition" + ssh $SCPARGS root@10.1.1.1 "umount /tmp/bootmnt" + + ssh $SCPARGS root@10.1.1.1 "rm -rf /tmp/bootmnt" + + echo "done" + +fi + + diff --git a/tools/updater/.gitignore b/tools/updater/.gitignore new file mode 100644 index 0000000..4799d15 --- /dev/null +++ b/tools/updater/.gitignore @@ -0,0 +1 @@ +*.tar.* \ No newline at end of file diff --git a/tools/updater/fetchLatest.sh b/tools/updater/fetchLatest.sh new file mode 100755 index 0000000..250b177 --- /dev/null +++ b/tools/updater/fetchLatest.sh @@ -0,0 +1,46 @@ +#!/bin/sh + +# Given a project directory (where "build" normally is), copy the latest +# update images from: +# build/tmp*/deploy/images/imx6solo_3dr/3dr-solo*.tar.gz +# build/tmp*/deploy/images/imx6solo_3dr_artoo/3dr-controller*.tar.gz +# renaming them in the copy with the supplied version number, e.g. +# controller_x.y.z.tar.gz +# solo_x.y.z.tar.gz +# +# $1 is the project directory +# $2 is the version (x.y.z) + +if [ $# -eq 0 ]; then + echo "Usage: $0 " + echo " where the images are in /build/tmp*/deploy/images" + exit +fi + +if [ -z "${2}" ]; then + version=`date +%Y%m%d` +else + version=${2} +fi + +image_dir=$1/build/tmp*/deploy/images + +get_image() { + # $1 is solo or controller + # $2 is link_name + # $3 is version + if [ -e ${2} ]; then + full_path=`readlink ${2}` + src_name=`basename ${full_path}` + dst_name=${1}_${3}.tar.gz + echo "copying ${src_name} -> ${dst_name}" + cp ${full_path} ${dst_name} + cp ${full_path}.md5 ${dst_name}.md5 + sed -i "s/${src_name}/${dst_name}/g" ${dst_name}.md5 + else + echo "${2} not found" + fi +} + +get_image solo ${image_dir}/imx6solo_3dr_1080p/3dr-solo-test.tar.gz ${version} +get_image controller ${image_dir}/imx6solo_3dr_artoo/3dr-controller-test.tar.gz ${version} diff --git a/tools/updater/makeGolden.sh b/tools/updater/makeGolden.sh new file mode 100755 index 0000000..107446f --- /dev/null +++ b/tools/updater/makeGolden.sh @@ -0,0 +1,31 @@ +#!/bin/sh + +#This script takes an updated image and pushes it +#on to the GOLDEN partition of the SD card, creating a +#new golden image. This should only be used for +#development and production. + +#Make sure we're running on the update partition +BOOTPART=`grep 'boot' /proc/mounts | awk '{print $1}'` + +if [ $BOOTPART == "/dev/mmcblk0p1" ]; then + echo "Already on golden partition" + exit +else + echo "Making the update partition the golden partition" +fi; + +echo "Unmounting the golden partition" +umount /dev/mmcblk0p1 + +echo "Creating the golden partition filesystem" +mkfs.vfat /dev/mmcblk0p1 -n GOLDEN + +echo "Mounting GOLDEN and copying files from LATEST" +mkdir -p golden +mount /dev/mmcblk0p1 golden +cp -r /mnt/boot/* golden/ +umount golden + +echo "All done! Run a factory reset if you'd like." + diff --git a/tools/updater/unpair b/tools/updater/unpair new file mode 100755 index 0000000..4968432 --- /dev/null +++ b/tools/updater/unpair @@ -0,0 +1,27 @@ +#!/bin/bash + +ssh_args="-o StrictHostKeyChecking=no -i ./updater_id_rsa" + + +solo_ip=10.1.1.10 + +echo -n "Unpairing Solo..." +while ! ping -c1 -W1 $solo_ip &> /dev/null; do echo -n "."; done + +sh-keygen -R $solo_ip &> /dev/null + +ssh $ssh_args root@$solo_ip 'cp /mnt/rootfs.ro/etc/wpa_supplicant.orig /etc/wpa_supplicant.conf; md5sum /etc/wpa_supplicant.conf > /etc/wpa_supplicant.conf.md5; sync; reboot' + +echo "OK" + + +artoo_ip=10.1.1.1 + +echo -n "Unpairing Artoo..." +while ! ping -c1 -W1 $artoo_ip &> /dev/null; do echo -n "."; done + +sh-keygen -R $artoo_ip &> /dev/null + +ssh $ssh_args root@$artoo_ip 'rm -f /log/3dr-pairing.conf; sync; reboot' + +echo "OK" diff --git a/tools/updater/updateFuncs b/tools/updater/updateFuncs new file mode 100755 index 0000000..7aee565 --- /dev/null +++ b/tools/updater/updateFuncs @@ -0,0 +1,198 @@ +# Intended to be sourced by other scripts + +export SOLO_IP=10.1.1.10 +export ARTOO_IP=10.1.1.1 + +SCP="scp -o StrictHostKeyChecking=no ${RSA_KEY}" +SSH="ssh -o StrictHostKeyChecking=no ${RSA_KEY}" + +sl_init_environment() { + system=`uname -a | awk '{print $1}'` + if [ $system == "Darwin" ]; then + if [ ! -e /usr/local/bin/md5sum ] && [ ! -e /opt/local/bin/md5sum ]; then + echo "md5sum missing: please install md5sha1sum using macports or brew" + return 1 + fi + export MD5SUM='md5sum --check' + else + export MD5SUM='md5sum --check --quiet' + fi + return 0 +} + +# sl_start_update [ip] [filename] [message] +# print message, then start update of 'ip' with 'filename' +# sl_start_update [ip] [filename] +# print default message, then start update of 'ip' with 'filename' +# sl_start_update [ip] [filename] - +# silently start update of 'ip' with 'filename' +sl_start_update() { + ${MD5SUM} ${2}.md5 > /dev/null 2>&1 + if [ $? -ne 0 ]; then + echo "ERROR checking md5 for ${2}.md5" + exit + fi + msg="updating ${1} with ${2}" + if [ "${3}_" != "_" ]; then + msg=${3} + fi + if [ "${msg}" != "-" ]; then + echo ${msg} + fi + ssh-keygen -R ${1} &> /dev/null + ${SSH} root@${1} "rm -rf /log/updates" &> /dev/null + ${SSH} root@${1} "mkdir -p /log/updates" &> /dev/null + ${SSH} root@${1} "touch /log/updates/UPDATE" &> /dev/null + ${SCP} ${2} root@${1}:/log/updates/ + ${SCP} ${2}.md5 root@${1}:/log/updates/ + ${SSH} root@${1} "shutdown -r now" +} + + +# sl_start_makegolden [ip] [message] +# print message, then start makegold on 'ip' +# sl_start_makegolden [ip] +# print default message, then start makegold on 'ip' +# sl_start_makegolden [ip] - +# silently start makegold on 'ip' +sl_start_makegolden() { + msg="makegold on ${1}" + if [ "${2}_" != "_" ]; then + msg=${2} + fi + if [ "${msg}" != "-" ]; then + echo ${msg} + fi + ssh-keygen -R ${1} &> /dev/null + ${SCP} ./makeGolden.sh root@${1}: &> /dev/null + ${SSH} root@${1} "chmod +x ./makeGolden.sh" + ${SSH} root@${1} "./makeGolden.sh" +} + + +# sl_start_factory [ip] [message] +# print message, then start factory reset on 'ip' +# sl_start_factory [ip] +# print default message, then start factory reset on 'ip' +# sl_start_factory [ip] - +# silently start factory reset on 'ip' +sl_start_factory() { + msg="starting factory reset on ${1}" + if [ "${2}_" != "_" ]; then + msg=${2} + fi + if [ "${msg}" != "-" ]; then + echo ${msg} + fi + ssh-keygen -R ${1} &> /dev/null + ${SSH} root@${1} "mkdir -p /log/updates" &> /dev/null + ${SSH} root@${1} "touch /log/updates/FACTORYRESET" &> /dev/null + ${SSH} root@${1} "shutdown -r now" +} + + +# sl_wait_target [ip] [message] +# print message, then dots until ip is pingable, then ok +# sl_wait_target [ip] +# print default message, then dots until ip is pingable, then ok +# sl_wait_target [ip] - +# silently wait until ip is pingable +sl_wait_target() { + msg="waiting for ${1}" + if [ "${2}_" != "_" ]; then + msg=${2} + fi + if [ "${msg}" != "-" ]; then + echo -n "${msg} ..." + fi + # wait for it to be pingable (network up) + while ! ping -c1 -W1 ${1} &> /dev/null; do + if [ "${msg}" != "-" ]; then echo -n "."; fi + done + # wait for it to be accepting ssh connections + while ! ${SSH} root@${1} "true" &> /dev/null; do + sleep 1 + if [ "${msg}" != "-" ]; then echo -n "-"; fi + done + # wait for it to be in runlevel 3, 4, or 5 + sl_get_runlevel ${1} + runlevel=$? + until [ "${runlevel}_" == "3_" ] || [ "${runlevel}_" == "4_" ] || [ "${runlevel}_" == "5_" ]; do + sleep 1 + sl_get_runlevel ${1} + if [ "${msg}" != "-" ]; then echo -n "+"; fi + done + if [ "${msg}" != "-" ]; then echo " ok"; fi +} + + +# sl_get_runlevel [ip] +sl_get_runlevel() { + rl=`${SSH} root@${1} 'runlevel'` + for w in ${rl}; do + a=${w} + done + return ${a} +} + + +# sl_check_update_files [filename] ... +sl_check_update_files() { + result=0 + if [ -z "${MD5SUM}" ]; then + echo "MD5SUM not set in environment" + result=1 + fi + until [ -z "${1}" ]; do + if [ ! -f "${1}" ]; then + echo "${1} missing" + result=1 + elif [ ! -f "${1}.md5" ]; then + echo "${1}.md5 missing" + result=1 + elif [ ! -z "${MD5SUM}" ]; then + ${MD5SUM} ${1}.md5 > /dev/null 2>&1 + if [ $? -ne 0 ]; then + echo "${1}.md5 incorrect" + result=1 + fi + fi + shift + done + return ${result} +} + + +# sl_get_version [ip] [which] +sl_get_version() { + echo `${SSH} root@${1} "cat /${2} | head -n 1"` +} + + +# sl_clean_firmware [ip] +# forces loading firmware on next boot if the build's version is different +# from what is already loaded +sl_clean_firmware() { + ssh-keygen -R ${1} &> /dev/null + ${SSH} root@${1} "rm -rf /mnt/rootfs.rw/firmware" &> /dev/null +} + + +# sl_start_update_pixhawk [ip] [filename] [message] +sl_start_update_pixhawk() { + msg="starting update of pixhawk to ${2} ... " + if [ "${3}_" != "_" ]; then + msg=${3} + fi + if [ "${msg}" != "-" ]; then + echo -n "${msg}" + fi + + ssh-keygen -R ${1} &> /dev/null + ${SCP} ${2} root@${1}:/firmware/ &> /dev/null + ${SSH} root@${1} "shutdown -r now" + + if [ "${msg}" != "-" ]; then + echo "ok" + fi +} diff --git a/tools/updater/updateSoloAndArtoo.sh b/tools/updater/updateSoloAndArtoo.sh new file mode 100755 index 0000000..54468a6 --- /dev/null +++ b/tools/updater/updateSoloAndArtoo.sh @@ -0,0 +1,651 @@ +#!/bin/bash + +CLEAR=clear + +SOLO_IP=10.1.1.10 +ARTOO_IP=10.1.1.1 + +SCPARGS="-o StrictHostKeyChecking=no -i ./updater_id_rsa" + +SERVERADDR="REDACTED" + +system=`uname -a | awk '{print $1}'` + +downloadTarAndMD5() { + FILEPATH=$1 + FILES=`curl -u sololink:redacted -s "${SERVERADDR}${FILEPATH}update/" | grep href | sed 's/.*href="//' | sed 's/".*//' | grep 'tar.gz$' | sed 's/\b\([0-9]\)\b/0\1/g' | sort | sed 's/\b0\([0-9]\)/\1/g'` + + for f in $FILES; do + latestFile=$f + done + + if [ -e $latestFile ] && [ -e ${latestFile}.md5 ]; then + echo "Latest file is already downloaded." + return + fi + + echo "Downloading file" $latestFile "and its MD5 sum" + curl --progress-bar -u sololink:redacted -O "${SERVERADDR}${FILEPATH}update/$latestFile" + curl --progress-bar -u sololink:redacted -s -O "${SERVERADDR}${FILEPATH}update/${latestFile}.md5" +} + +pingWait() { + address=$1 + + if [[ $system =~ ^MINGW32.* ]]; then + while ! ping -n 1 $address &> /dev/null; do sleep 1; echo -n "."; done + return + else + while ! ping -c1 -W1 $address &> /dev/null; do sleep 1; echo -n "."; done + return + fi +} + +copyUpdate() { + address=$1 + tarball=$2 + md5file=$3 + + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + #Copy the update files to the target + echo "Sending update file to $address" + ssh-keygen -R $address &> /dev/null + ssh $SCPARGS root@$address "sololink_config --update-prepare sololink" &> /dev/null + # older versions don't have sololink-config and ssh returns 127 + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "rm -rf /log/updates" &> /dev/null + ssh $SCPARGS root@$address "mkdir -p /log/updates" &> /dev/null + fi + scp $SCPARGS ${tarball} root@$address:/log/updates/ + scp $SCPARGS ${md5file} root@$address:/log/updates/ +} + +startUpdate() { + address=$1 + reset=$2 + + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + echo "Starting update on $address" + ssh-keygen -R $address &> /dev/null + if [ $reset == 1 ]; then + ssh $SCPARGS root@$address "sololink_config --update-apply sololink --reset" &> /dev/null + else + ssh $SCPARGS root@$address "sololink_config --update-apply sololink" &> /dev/null + fi + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "touch /log/updates/UPDATE" &> /dev/null + ssh $SCPARGS root@$address "shutdown -r now" + fi +} + +checkMD5() { + thisfile=$1 + + # require corresponding md5 file + if [ ! -e ${thisfile}.md5 ]; then + echo "No ${thisfile}.md5, please place one in this directory." + exit + fi + # check md5 before upload + if [[ $system == 'Darwin' ]] || [[ $system =~ ^MINGW32.* ]]; then + md5sum --check ${thisfile}.md5 > /dev/null 2>&1 + else + md5sum --check --quiet ${thisfile}.md5 > /dev/null 2>&1 + fi + # $?==0 on success, !=0 on failure + if [ $? -ne 0 ]; then + echo "ERROR checking md5 for ${thisfile}.md5" + cat ${thisfile}.md5 + exit + fi +} + +$CLEAR + +if [ $system == "Darwin" ]; then + if [ ! -e /usr/local/bin/md5sum ] && [ ! -e /opt/local/bin/md5sum ]; then + echo "Please install md5sha1sum using macports or brew" + exit + fi +fi + +menu() { + echo "SoloLink Update, Reset and Configuration" + echo "" + echo "Commands:" + echo "" + echo " i.MX6/System updates" + echo " update " + echo " reset " + echo " makegold " + echo "" + echo " Firmware updates" + echo " pixupdate" + echo " stm32update" + echo "" + echo " Configuration options" + echo " setSSID " + echo "" + echo "Type any command followed by \"help\" for more information" + while true; do + read -p "Please enter a command (q to exit):" cmd + args=`echo $cmd | awk '{$1=""; print $0}'` + case $cmd in + update* ) runUpdate $args; exit;; + reset* ) runReset $args; exit;; + makegold* ) runMakeGold $args; exit;; + pixupdate* ) runUpdatePixhawk $args; exit;; + stm32update* ) runUpdateSTM32 $args; exit;; + setSSID* ) runSetSSID $args; exit;; + q|quit|exit ) exit;; + * ) echo "Please enter a valid command."; sleep 1; menu;; + esac + done +} + +runUpdate() { + if [ -z $1 ]; then + echo "Invalid arguments. Usage:" + echo " update " + menu + fi + + artooonly=0 + soloonly=0 + if [ $1 == "solo" ]; then + soloonly=1 + elif [ $1 == "artoo" ]; then + artooonly=1 + elif [ $1 == "both" ]; then + artooonly=0 + soloonly=0 + elif [ $1 == "help" ]; then + echo "Performs an update of the Solo, Artoo, or both. Software will not be" + echo "automatically downloaded unless the download argument is specified." + echo "The reset argument will perform a settings reset with the update, putting" + echo "the Solo and Artoo in a default state after the update finishes" + echo "The total software update will take between 2 and 5 minutes, depending" + echo "on the internet connection and the number of components on the Solo/Artoo" + echo "that require updating." + echo "" + menu + else + echo "Invalid arguments. Usage:" + echo " update " + menu + fi + + noDL=1 + reset=0 + if [ "_$2" == "_download" ]; then + noDL=0 + if [ "_$3" == "_reset" ]; then + reset=1 + fi + fi + + if [ "_$2" == "_reset" ]; then + reset=1 + fi + + $CLEAR + + if [ $noDL == 0 ]; then + echo -n "Waiting for internet connectivity" + pingWait "google.com" + $CLEAR + fi + + if [ $artooonly == 0 ]; then + if [ $noDL == 0 ]; then + # Check and/or download the latest update file + echo "Checking for latest Solo software" + downloadTarAndMD5 "/solo/1080p/" + fi + + solofile="" + # find the last file that matches *solo*.tar.gz + for f in `ls *solo*.tar.gz`; do + solofile=$f + done + + checkMD5 $solofile + fi + + if [ $soloonly == 0 ]; then + if [ $noDL == 0 ]; then + # Check and/or download the latest update file + echo "Checking for latest Artoo software" + downloadTarAndMD5 "/artoo/digital/" + fi + + artoo="" + # find the last file that matches *controller*.tar.gz + for f in `ls *controller*.tar.gz`; do + artoofile=$f + done + + checkMD5 $artoofile + fi + + $CLEAR + + if [ $soloonly == 1 ]; then + echo "Please power-up Solo and connect to the SoloLink network" + elif [ $artooonly == 1 ]; then + echo "Please power-up Artoo and connect to the SoloLink network" + else + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + fi + + if [ $artooonly == 0 ]; then + copyUpdate $SOLO_IP $solofile "${solofile}.md5" + fi + + if [ $soloonly == 0 ]; then + copyUpdate $ARTOO_IP $artoofile "${artoofile}.md5" + fi + + if [ $artooonly == 0 ]; then + startUpdate $SOLO_IP $reset + fi + + if [ $soloonly == 0 ]; then + startUpdate $ARTOO_IP $reset + fi + + echo "Please wait at least three minutes for the update to complete." +} + +runReset() { + + if [ -z $1 ]; then + echo "Invalid arguments. Usage:" + echo " reset " + menu + fi + + artooonly=0 + soloonly=0 + if [ $1 == "solo" ]; then + soloonly=1 + ADDRESSES="$SOLO_IP" + elif [ $1 == "artoo" ]; then + artooonly=1 + ADDRESSES="$ARTOO_IP" + elif [ $1 == "both" ]; then + artooonly=0 + soloonly=0 + ADDRESSES="$SOLO_IP $ARTOO_IP" + elif [ $1 == "help" ]; then + echo "Performs either a factory reset or a settings reset of the" + echo "Solo, Artoo or both." + echo "" + echo "A factory reset resets to the \"Golden\" image stored in the" + echo "SD card of the i.MX6. This reverts all user settings and" + echo "reverts the kernel, devicetree and rootfs to their original" + echo "factory defaults. Any prior updates are overridden, and the" + echo "logging partition is erased." + echo "" + echo "A settings reset removes any user settings and erases the " + echo "logging partition. The latest update is preserved, but the" + echo "system reverts to a default state and will need to be" + echo "re-paired and re-configured by the user." + echo "" + menu + else + echo "Invalid arguments. Usage:" + echo " reset " + menu + fi + + if [ -z $2 ]; then + echo "Invalid arguments. Usage:" + echo " reset " + menu + fi + + $CLEAR + + if [ $2 == "factory" ]; then + mode="factory" + elif [ $2 == "settings" ]; then + mode="settings" + else + echo "Invalid arguments. Usage:" + echo " reset " + menu + fi + + echo -n "Performing a $mode reset on" + if [ $artooonly == 1 ]; then + echo " Artoo" + elif [ $soloonly == 1 ]; then + echo " Solo" + else + echo " both Solo and Artoo" + fi + + while true; do + read -p "Are you sure you would like to proceed(y/n)?" yn + case $yn in + [Yy]* ) break;; + [Nn]* ) $CLEAR; menu;; + * ) echo "Please answer yes or no.";; + esac + done + + if [ $soloonly == 1 ]; then + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + elif [ $artooonly == 1 ]; then + echo "Please power-up Artoo and connect to the SoloLink network" + else + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + fi + + for address in $ADDRESSES; do + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + echo "Starting $mode reset on $address" + ssh-keygen -R $address &> /dev/null + if [ $mode == "factory" ]; then + ssh $SCPARGS root@$address "sololink_config --factory-reset" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "mkdir -p /log/updates" &> /dev/null + ssh $SCPARGS root@$address "touch /log/updates/FACTORYRESET" &> /dev/null + ssh $SCPARGS root@$address "shutdown -r now" + fi + else + ssh $SCPARGS root@$address "sololink_config --settings-reset" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "mkdir -p /log/updates" &> /dev/null + ssh $SCPARGS root@$address "touch /log/updates/RESETSETTINGS" &> /dev/null + ssh $SCPARGS root@$address "shutdown -r now" + fi + fi + + done + + echo "Please wait at least 2 minutes for the $mode reset to complete" +} + +runMakeGold() { + if [ -z $1 ]; then + echo "Invalid arguments. Usage:" + echo " makegold " + menu + fi + + artooonly=0 + soloonly=0 + if [ $1 == "solo" ]; then + soloonly=1 + ADDRESSES="$SOLO_IP" + elif [ $1 == "artoo" ]; then + artooonly=1 + ADDRESSES="$ARTOO_IP" + elif [ $1 == "both" ]; then + artooonly=0 + soloonly=0 + ADDRESSES="$SOLO_IP $ARTOO_IP" + elif [ $1 == "help" ]; then + echo "Makes the current updated image on the i.MX6 the \"Golden\" image" + echo "on the SD card. This way, when a future factory reset is performed" + echo "the system reverts to the image that is currently running instead" + echo "of the older \"Golden\" image. The original \"Golden\" image is" + echo "overwritten. This is useful for development boards, as it is" + echo "likely that the older \"Golden\" image will become obsolete." + echo "" + menu + else + echo "Invalid arguments. Usage:" + echo " makegold " + menu + fi + + $CLEAR + + echo -n "Overwriting the \"Golden\" partition on" + if [ $artooonly == 1 ]; then + echo " Artoo" + elif [ $soloonly == 1 ]; then + echo " Solo" + else + echo " both Solo and Artoo" + fi + + while true; do + read -p "Are you sure you would like to proceed(y/n)?" yn + case $yn in + [Yy]* ) break;; + [Nn]* ) $CLEAR; menu;; + * ) echo "Please answer yes or no.";; + esac + done + + if [ $soloonly == 1 ]; then + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + elif [ $artooonly == 1 ]; then + echo "Please power-up Artoo and connect to the SoloLink network" + else + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + fi + + for address in $ADDRESSES; do + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + echo "Starting make golden on $address" + ssh-keygen -R $address &> /dev/null + ssh $SCPARGS root@$address "sololink_config --make-golden" &> /dev/null + if [ $? -ne 0 ]; then + scp $SCPARGS ./makeGolden.sh root@$address: &> /dev/null + ssh $SCPARGS root@$address "chmod +x ./makeGolden.sh" + ssh $SCPARGS root@$address "./makeGolden.sh" + fi + done +} + +runUpdatePixhawk() { + if [ $1 == "help" ]; then + echo "Updates the Pixhawk on Solo with the Arducopter-X.X.X.px4 file" + echo "stored in this folder. The ArduCopter-X.X.X.px4 file must have" + echo "been previously downloaded and placed here." + echo "" + menu + fi + + $CLEAR + + echo "Checking for pixhawk update file." + # find the last file that matches ArduCopter*.px4 + if [ ! -e ArduCopter-*.*.*.px4 ]; then + echo "Error, no ArduCopter-*.*.*.px4 found. Please download and retry" + exit + fi + + for f in `ls ArduCopter-*.*.*.px4`; do + pixfile=$f + done + + echo "Updating the pixhawk with $pixfile" + + while true; do + read -p "Are you sure you would like to proceed(y/n)?" yn + case $yn in + [Yy]* ) break;; + [Nn]* ) $CLEAR; menu;; + * ) echo "Please answer yes or no.";; + esac + done + + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + + address="10.1.1.10" + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + echo "Starting pixhawk update on $address" + ssh-keygen -R $address &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --update-prepare pixhawk" &> /dev/null + # okay if that returns an error + + scp $SCPARGS $pixfile root@$address:/firmware/ &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --update-apply pixhawk" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "shutdown -r now" + fi +} + +runUpdateSTM32() { + if [ $1 == "help" ]; then + echo "Updates the STM32 on the Artoo with the artoo_x.x.x.bin file" + echo "stored in this folder. The artoo_x.x.x.bin file must have" + echo "been previously downloaded and placed here." + echo "" + menu + fi + + $CLEAR + + echo "Checking for artoo update file." + # find the last file that matches artoo + if [ ! -e artoo_*.bin ]; then + echo "Error, no artoo_x.x.x.bin found. Please download and retry" + exit + fi + + for f in `ls artoo_*.bin`; do + artoofile=$f + done + + echo "Updating the Artoo with $artoofile" + + while true; do + read -p "Are you sure you would like to proceed(y/n)?" yn + case $yn in + [Yy]* ) break;; + [Nn]* ) $CLEAR; menu;; + * ) echo "Please answer yes or no.";; + esac + done + + echo "Please power-up Artoo and connect to the SoloLink network" + + address="10.1.1.1" + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + echo "Starting artoo update on $address" + ssh-keygen -R $address &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --update-prepare artoo" &> /dev/null + # okay if that returns an error + + scp $SCPARGS $artoofile root@$address:/firmware/${artoofile} &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --update-apply artoo" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "init 2" + ssh $SCPARGS root@$address "checkArtooAndUpdate.py" + ssh $SCPARGS root@$address "init 3" + fi +} + +runSetSSID() { + if [ -z $1 ]; then + echo "Invalid arguments. Usage:" + echo " setSSID " + menu + fi + + if [ $1 == "help" ]; then + echo "Sets the SSID of the currently connected Artoo. After setting" + echo "the SSID, the ArtooLink reboots. You will be required to re-pair" + echo "your Solo to the new SSID and re-connect your computer if necessary" + echo "" + menu + fi + + echo "I will be renaming the Artoo SSID to SoloLink_${1}." + while true; do + read -p "Are you sure you would like to proceed (y/n)?" yn + case $yn in + [Yy]* ) break;; + [Nn]* ) menu; break;; + * ) echo "Please answer yes or no.";; + esac + done + + $CLEAR + + echo "Please power-up Solo and Artoo and connect to the SoloLink network" + address="10.1.1.1" + #Wait for the target to come up + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + ssh-keygen -R $address &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --set-wifi-ssid SoloLink_${1}" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "sed -i \"s/^ssid=SoloLink_.*/ssid=SoloLink_${1}/\" /etc/hostapd.conf" + ssh $SCPARGS root@$address "md5sum /etc/hostapd.conf > /etc/hostapd.conf.md5" + fi + + while true; do + read -p "Would you like to set the SSID on Solo as well (y/n)? Otherwise you will have to re-pair your Solo." yn + case $yn in + [Yy]* ) break;; + [Nn]* ) + ssh $SCPARGS root@$address "shutdown -r now"; + echo "The Artoolink (and SoloLink) will now reboot. Please re-pair your Solo after it"; + echo "has finished rebooting, which will take about 30s."; + exit;; + * ) echo "Please answer yes or no.";; + esac + done + + address="10.1.1.10" + echo "Setting SSID on Solo." + echo -n "Waiting to connect to $address" + pingWait $address + echo "Connected to $address" + + ssh-keygen -R $address &> /dev/null + + ssh $SCPARGS root@$address "sololink_config --set-wifi-ssid SoloLink_${1}" &> /dev/null + if [ $? -ne 0 ]; then + ssh $SCPARGS root@$address "sed -i \"s/ssid=\\\"SoloLink_.*/ssid=\\\"SoloLink_${1}\\\"/\" /etc/wpa_supplicant.conf" + ssh $SCPARGS root@$address "md5sum /etc/wpa_supplicant.conf > /etc/wpa_supplicant.conf.md5" + fi + + ssh $SCPARGS root@$address "shutdown -r now" + ssh $SCPARGS root@10.1.1.1 "shutdown -r now" + + echo "The Artoolink & SoloLink will now reboot. Please wait at least 30s for them to re-connect" + +} + +$CLEAR +menu diff --git a/tools/updater/updateTest b/tools/updater/updateTest new file mode 100755 index 0000000..6ce31a8 --- /dev/null +++ b/tools/updater/updateTest @@ -0,0 +1,101 @@ +#!/bin/bash + +source updateFuncs + +ARTOO_FILE_1=3dr-controller-20150316023643.tar.gz +SOLO_FILE_1=3dr-solo-20150316023855.tar.gz +SOLOLINK_VER_1=0.6.4 +PIXHAWK_VER_1=0.0.26 +STM32_VER_1=0.6.7 + +ARTOO_FILE_2=3dr-controller-20150316024635.tar.gz +SOLO_FILE_2=3dr-solo-20150316024848.tar.gz +SOLOLINK_VER_2=0.7.0 +PIXHAWK_VER_2=0.0.27 +STM32_VER_2=0.6.8 + +if ! sl_init_environment; then + echo "can't continue" + exit +fi + +if ! sl_check_update_files ${SOLO_FILE_1} ${ARTOO_FILE_1} ${SOLO_FILE_2} ${ARTOO_FILE_2}; then + echo "can't continue" + exit +fi + +show_versions() { + echo -n "Controller: " + echo -n "SoloLink `sl_get_version 10.1.1.1 VERSION` " + echo "STM32 `sl_get_version 10.1.1.1 STM_VERSION`" + echo -n "Solo: " + echo -n "SoloLink `sl_get_version 10.1.1.10 VERSION` " + echo "Pixhawk `sl_get_version 10.1.1.10 PIX_VERSION`" +} + +# check_versions [sololink_version] [stm32_version] [pixhawk_version] +check_versions() { + artoo_version=`sl_get_version 10.1.1.1 VERSION` + stm32_version=`sl_get_version 10.1.1.1 STM_VERSION` + solo_version=`sl_get_version 10.1.1.10 VERSION` + pixhawk_version=`sl_get_version 10.1.1.10 PIX_VERSION` + result=0 + if [ ${artoo_version} != ${1} ]; then + echo "artoo sololink is ${artoo_version}; expected ${1}" + result=1 + fi + if [ ${solo_version} != ${1} ]; then + echo "solo sololink is ${solo_version}; expected ${1}" + result=1 + fi + if [ ${stm32_version} != ${2} ]; then + echo "artoo stm32 is ${stm32_version}; expected ${2}" + result=1 + fi + if [ ${pixhawk_version} != ${3} ]; then + echo "solo pixhawk is ${pixhawk_version}; expected ${3}" + result=1 + fi + if [ ${result} -eq 0 ]; then + echo "versions correct" + fi + return ${result} +} + +SCPARGS="-o StrictHostKeyChecking=no -i ./updater_id_rsa" + +sl_wait_target ${SOLO_IP} + +# start toggling back and forth + +update_count=0 + +while true; do + + sl_clean_firmware ${SOLO_IP} + sl_start_update ${SOLO_IP} ${SOLO_FILE_1} "updating solo to ${SOLO_FILE_1}" + sl_clean_firmware ${ARTOO_IP} + sl_start_update ${ARTOO_IP} ${ARTOO_FILE_1} "updating artoo to ${ARTOO_FILE_1}" + + sleep 10 + sl_wait_target ${SOLO_IP} "waiting for update to complete" + + check_versions ${SOLOLINK_VER_1} ${STM32_VER_1} ${PIXHAWK_VER_1} + + update_count=$((update_count+1)) + echo "${update_count} updates" + + sl_clean_firmware ${SOLO_IP} + sl_start_update ${SOLO_IP} ${SOLO_FILE_2} "updating solo to ${SOLO_FILE_2}" + sl_clean_firmware ${ARTOO_IP} + sl_start_update ${ARTOO_IP} ${ARTOO_FILE_2} "updating artoo to ${ARTOO_FILE_2}" + + sleep 10 + sl_wait_target ${SOLO_IP} "waiting for update to complete" + + check_versions ${SOLOLINK_VER_2} ${STM32_VER_2} ${PIXHAWK_VER_2} + + update_count=$((update_count+1)) + echo "${update_count} updates" + +done diff --git a/tools/wifiTest/ArtooTest.sh b/tools/wifiTest/ArtooTest.sh new file mode 100755 index 0000000..9971b5f --- /dev/null +++ b/tools/wifiTest/ArtooTest.sh @@ -0,0 +1,140 @@ +#!/bin/sh +clear + +echo "Artoo Test Script" +SSHARGS="-o StrictHostKeyChecking=no -o ConnectTimeout=3 -i updater_id_rsa" +MYMAC=`ifconfig -a | grep HWaddr | awk 'NR==1 {print $5}' | tr '[:upper:]' '[:lower:]'` +DEBUGFSDIR=/sys/kernel/debug/ieee80211/phy0/ + +connectToTF() { + mySSID=$1 + rm -f /tmp/wpa* + killall wpa_supplicant &> /dev/null + echo "Connecting to $mySSID" + cp /etc/wpa_supplicant.conf /tmp/ + #Set the wpa_supplicant.conf + wpa_passphrase ${mySSID} sololink >> /tmp/wpa_supplicant.conf + + # echo "network={" >> /tmp/wpa_supplicant.conf + # echo " ssid=\"${mySSID}\"" >> /tmp/wpa_supplicant.conf + # echo " key_mgmt=NONE" >> /tmp/wpa_supplicant.conf + # echo "}" >> /tmp/wpa_supplicant.conf + + wpa_supplicant -c/tmp/wpa_supplicant.conf -Dnl80211 -B -iwlan0 &> /dev/null + ifconfig wlan0 10.1.1.10 + ping 10.1.1.1 -c1 -W5 &> /dev/null + if [ $? -ne 0 ]; then + echo "Unable to connect" + echo "********************FAIL************************" + disconnectFromTF + exit 1 + fi +} + +disconnectFromTF() { + echo "Disconnecting" + ifconfig wlan0 down + killall wpa_supplicant &> /dev/null +} + +/etc/init.d/hostapd stop &> /dev/null +/etc/init.d/dnsmasq stop &> /dev/null +ifdown wlan0 &> /dev/null +ifdown wlan0-ap &> /dev/null + +while [ True ]; do + + echo "Please scan barcode or enter MAC address manually (ctrl-c to exit)" + read MAC + + #Parse the SSID from the MAC address + ID=`echo $MAC | tail -c 7` + SSID="SoloLink_$ID" + + echo "Initializing system" + init 2 + sleep 2 + ssh-keygen -R 10.1.1.1 &> /dev/null + connectToTF $SSID + #ssh $SSHARGS root@10.1.1.1 "iw phy0 set txpower fixed 100" + #iw phy0 set txpower fixed 100 + + echo "Testing connection" + PASS=1 + for ant in 1 2; do + echo "Antenna ${ant}: " + ifconfig wlan0 down + iw phy0 set antenna 0x${ant} 0x${ant} + ifconfig wlan0 10.1.1.10 + sleep 1 + rx=0 + tx=0 + rxWorst=0 + txWorst=0 + rxBest=-200 + txBest=-200 + for i in {1..20}; do + rxSig=`cat ${DEBUGFSDIR}netdev:wlan0/stations/*/last_signal` + txSig=`ssh $SSHARGS root@10.1.1.1 "cat \"${DEBUGFSDIR}netdev:wlan0-ap/stations/${MYMAC}/last_signal\""` + + if [ $? -ne 0 ]; then + echo "Unable to connect to read signal value" + echo "***************${SSID} FAIL************************" + PASS=0 + break + fi + + echo -n "." + #echo "$txSig $txWorst $txBest" + #echo "$rxSig $rxWorst $rxBest" + rx=$((rx+rxSig)) + tx=$((tx+txSig)) + if [ $rxSig -le $rxWorst ]; then + rxWorst=$rxSig + fi + if [ $txSig -le $txWorst ]; then + txWorst=$txSig + fi + if [ $rxSig -ge $rxBest ]; then + rxBest=$rxSig + fi + if [ $txSig -ge $txBest ]; then + txBest=$txSig + fi + done + echo "" + rx=$((rx/20)) + tx=$((tx/20)) + echo "Rx best: ${rxBest} avg: ${rx} worst: $rxWorst" + echo "Tx best: ${txBest} avg: ${tx} worst: $txWorst" + + if [ $rx -le -42 ] && [ $rxBest -le -40 ]; then + echo "*************${SSID} FAIL************************" + disconnectFromTF + #echo "Done" + #exit + PASS=0 + break + fi + + if [ $tx -le -42 ] && [ $rxBest -le -40 ]; then + echo "*************${SSID} FAIL************************" + disconnectFromTF + #echo "Done" + #exit + PASS=0 + break + fi + done + + if [ $PASS -eq 1 ]; then + echo "************${SSID} PASS************************" + disconnectFromTF + fi + + #iw phy0 set txpower fixed 2700 + #ssh $SSHARGS root@10.1.1.1 "iw phy0 set txpower fixed 2700" + #echo "Done" + +done + diff --git a/tools/wifiTest/SoloTest.sh b/tools/wifiTest/SoloTest.sh new file mode 100755 index 0000000..18bef69 --- /dev/null +++ b/tools/wifiTest/SoloTest.sh @@ -0,0 +1,124 @@ +#!/bin/sh +clear + +echo "Solo Test Script" +SSHARGS="-o StrictHostKeyChecking=no -o ConnectTimeout=3" +MAC=`ifconfig -a | grep HWaddr | awk '{print $5}' | tr '[:upper:]' '[:lower:]'` +DEBUGFSDIR=/sys/kernel/debug/ieee80211/phy0/ + +connectToTF() { + SSID=$1 + rm -f /tmp/wpa* + killall wpa_supplicant &> /dev/null + echo "Connecting" + cp /etc/wpa_supplicant.conf /tmp/ + wpa_passphrase $SSID sololink > /tmp/wpa_supplicant.conf + wpa_supplicant -c/tmp/wpa_supplicant.conf -Dnl80211 -B -iwlan0 &> /dev/null + ifconfig wlan0 10.1.1.10 + ping 10.1.1.1 -c1 -W5 &> /dev/null + if [ $? -ne 0 ]; then + echo "Unable to connect" + echo "********************FAIL************************" + disconnectFromTF + exit 1 + fi +} + +disconnectFromTF() { + echo "Disconnecting" + ifconfig wlan0 down + killall wpa_supplicant +} + +if [ "$#" -ne 1 ]; then + echo "Please specify the SoloLink_TEST network SSID" + exit 1 +fi + +TFSSID=$1 + +echo "Initializing system" +init 2 +sleep 2 +ssh-keygen -R 10.1.1.1 &> /dev/null + +echo "Attempting to connect to $TFSSID" +connectToTF $TFSSID +#ssh $SSHARGS root@10.1.1.1 "iw phy0 set txpower fixed 100" +#iw phy0 set txpower fixed 100 + +echo "Testing connection" +PASS=1 +for ant in 1 2; do + echo "Antenna ${ant}: " + ifconfig wlan0 down + iw phy0 set antenna 0x${ant} 0x${ant} + ifconfig wlan0 10.1.1.10 + sleep 1 + rx=0 + tx=0 + rxWorst=0 + txWorst=0 + rxBest=-200 + txBest=-200 + for i in {1..20}; do + rxSig=`cat ${DEBUGFSDIR}netdev:wlan0/stations/*/last_signal` + txSig=`ssh $SSHARGS root@10.1.1.1 "cat ${DEBUGFSDIR}netdev:wlan0-ap/stations/${MAC}/last_signal"` + if [ $? -ne 0 ]; then + echo "Unable to connect to read signal value." + echo "********************FAIL************************" + PASS=0 + break + fi + + echo -n "." + #echo "$txSig $txWorst $txBest" + #echo "$rxSig $rxWorst $rxBest" + rx=$((rx+rxSig)) + tx=$((tx+txSig)) + if [ $rxSig -le $rxWorst ]; then + rxWorst=$rxSig + fi + if [ $txSig -le $txWorst ]; then + txWorst=$txSig + fi + if [ $rxSig -ge $rxBest ]; then + rxBest=$rxSig + fi + if [ $txSig -ge $txBest ]; then + txBest=$txSig + fi + done + echo "" + rx=$((rx/20)) + tx=$((tx/20)) + echo "Rx best: ${rxBest} avg: ${rx} worst: $rxWorst" + echo "Tx best: ${txBest} avg: ${tx} worst: $txWorst" + + if [ $rx -le -42 ] && [ $rxBest -le -40 ]; then + echo "********************FAIL************************" + disconnectFromTF + #echo "Done" + #exit + PASS=0 + break + fi + + if [ $tx -le -42 ] && [ $rxBest -le -40 ]; then + echo "********************FAIL************************" + disconnectFromTF + #echo "Done" + #exit + PASS=0 + break + fi +done + +if [ $PASS -eq 1 ]; then + echo "********************PASS************************" + disconnectFromTF +fi +#iw phy0 set txpower fixed 2700 +#ssh $SSHARGS root@10.1.1.1 "iw phy0 set txpower fixed 2700" +echo "Done" + diff --git a/wifi/Makefile b/wifi/Makefile new file mode 100644 index 0000000..b39b41c --- /dev/null +++ b/wifi/Makefile @@ -0,0 +1,32 @@ +# To cross compile: +# +# Set up as usual for bitbake: +# $ . setup-environment build +# +# In the build directory: +# $ bitbake meta-ide-support +# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi +# +# Now a make in this directory should work. + +VPATH = ../flightcode/util + +INCROOT = $(OECORE_TARGET_SYSROOT)/usr/include +CFLAGS += -Wall + +BIN1 = wifistats +BIN2 = survey_dump + +all: $(BIN1) $(BIN2) + +wifistats: CFLAGS += -I../flightcode/util +wifistats: wifistats.o util.o + $(LINK.c) -o wifistats wifistats.o util.o + +survey_dump: CFLAGS += -I$(INCROOT)/libnl3 +survey_dump: LDLIBS += -lnl-3 -lnl-genl-3 + +clean: + $(RM) *.o *~ $(BIN1) $(BIN2) + +.PHONY: all clean diff --git a/wifi/logRCUp.sh b/wifi/logRCUp.sh new file mode 100755 index 0000000..110bbfb --- /dev/null +++ b/wifi/logRCUp.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +XMITFILE=/sys/kernel/debug/ieee80211/phy0/ath9k/xmit + +LASTBYTES=0 +while [ True ]; do + + #Scrape the number of transmitted bytes + + #Needs to handle output like this (colums run together): + #TX-Pkts-All: 4508487 0 784093 369638 + #TX-Bytes-All: 1050712988 01076297731 40691534 + #HW-put-tx-buf: 1050315 0 318996 368833 + # NF is the number of fields (normally 5, 4 when they run together + # $NF is the last field + + BYTES=`cat $XMITFILE | grep 'TX-Bytes-All' | awk '{ print $NF }'` + + #So we don't put out bogus data + if [ $LASTBYTES -ne 0 ]; then + + #Calculate the delta + DELTA=$((BYTES-LASTBYTES)) + + logger -t wifi -p local2.info -- "${DELTA}" + + fi + + LASTBYTES=$BYTES + + #Log every second + sleep 1 + +done diff --git a/wifi/survey_dump.c b/wifi/survey_dump.c new file mode 100644 index 0000000..0275604 --- /dev/null +++ b/wifi/survey_dump.c @@ -0,0 +1,313 @@ +#include +#include +#include +#include +#include +#include + +/* +* Retrieve wifi scan survey results and print to stdout +* +* Netlink documentation: http://www.infradead.org/~tgr/libnl/ +* Netlink example: iw +*/ + +static int survey_handler(struct nl_msg *msg, void *arg); +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, + void *arg); +static int finish_handler(struct nl_msg *msg, void *arg); +static int ack_handler(struct nl_msg *msg, void *arg); + +static int header = 0; + + +int main(int argc, char *argv[]) +{ + volatile int status = 0; + struct nl_sock *nl_sock = NULL; + int nl80211_id = -1; + unsigned int if_index = 0; + uint32_t if_index_uint32; + struct nl_msg *msg = NULL; + struct nl_cb *cmd_cb = NULL; + struct nl_cb *sock_cb = NULL; + void *hdr = NULL; + int num_bytes = -1; + + nl_sock = nl_socket_alloc(); + if (nl_sock == NULL) { + fprintf(stderr, "ERROR allocating netlink socket\n"); + status = 1; + goto cleanup; + } + + if (genl_connect(nl_sock) != 0) { + fprintf(stderr, "ERROR connecting netlink socket\n"); + status = 1; + goto cleanup; + } + + nl80211_id = genl_ctrl_resolve(nl_sock, "nl80211"); + if (nl80211_id < 0) { + fprintf(stderr, "ERROR resolving netlink socket\n"); + status = 1; + goto cleanup; + } + + if_index = if_nametoindex("wlan0-ap"); + if (if_index == 0) { + fprintf(stderr, "ERROR getting interface index\n"); + status = 1; + goto cleanup; + } + + msg = nlmsg_alloc(); + if (msg == NULL) { + fprintf(stderr, "ERROR allocating netlink message\n"); + status = 1; + goto cleanup; + } + + cmd_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (cmd_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink command callback\n"); + status = 1; + goto cleanup; + } + + sock_cb = nl_cb_alloc(NL_CB_DEFAULT); + if (sock_cb == NULL) { + fprintf(stderr, "ERROR allocating netlink socket callback\n"); + status = 1; + goto cleanup; + } + + hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, nl80211_id, 0, + NLM_F_DUMP, NL80211_CMD_GET_SURVEY, 0); + if (hdr == NULL) { + fprintf(stderr, "ERROR creating netlink message\n"); + status = 1; + goto cleanup; + } + + if_index_uint32 = if_index; + if (nla_put(msg, NL80211_ATTR_IFINDEX, sizeof(uint32_t), &if_index_uint32) + != 0) { + fprintf(stderr, "ERROR setting message attribute\n"); + status = 1; + goto cleanup; + } + + if (nl_cb_set(cmd_cb, NL_CB_VALID, NL_CB_CUSTOM, survey_handler, NULL) + != 0) { + fprintf(stderr, "ERROR setting command callback\n"); + status = 1; + goto cleanup; + } + + nl_socket_set_cb(nl_sock, sock_cb); + + num_bytes = nl_send_auto_complete(nl_sock, msg); + if (num_bytes < 0) { + fprintf(stderr, "ERROR sending netlink message\n"); + status = 1; + goto cleanup; + } + + status = 1; + nl_cb_err(cmd_cb, NL_CB_CUSTOM, error_handler, (void *)&status); + nl_cb_set(cmd_cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, + (void *)&status); + nl_cb_set(cmd_cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, (void *)&status); + + while (status == 1) + nl_recvmsgs(nl_sock, cmd_cb); + +cleanup: + + if (sock_cb != NULL) + nl_cb_put(sock_cb); + + if (cmd_cb != NULL) + nl_cb_put(cmd_cb); + + if (msg != NULL) + nlmsg_free(msg); + + if (nl_sock != NULL) + nl_socket_free(nl_sock); + + return status; + +} /* main */ + + +static struct { + int chan; + int freq; +} freq_chan_map[] = { + {1, 2412}, {2, 2417}, {3, 2422}, {4, 2427}, {5, 2432}, {6, 2437}, + {7, 2442}, {8, 2447}, {9, 2452}, {10, 2457}, {11, 2462}, {12, 2467}, + {13, 2472}, {14, 2484}, +}; + + +static int freq_to_chan(int freq) +{ + int i_max = sizeof(freq_chan_map) / sizeof(freq_chan_map[0]); + int i; + + for (i = 0; i < i_max; i++) + if (freq == freq_chan_map[i].freq) + return freq_chan_map[i].chan; + + return -1; +} + + +/* Called for each survey result message that comes back from the kernel */ +static int survey_handler(struct nl_msg *msg, void *arg) +{ + struct nlattr *tb1[NL80211_ATTR_MAX + 1]; + struct nlmsghdr *nl_hdr; + struct genlmsghdr *genl_hdr; + struct nlattr *genl_attr_data; + int genl_attr_len; + unsigned int if_index; + char if_name[IF_NAMESIZE + 1]; + struct nlattr *tb2[NL80211_SURVEY_INFO_MAX + 1]; + static struct nla_policy policy[NL80211_SURVEY_INFO_MAX + 1] = { { 0 } }; + + policy[NL80211_SURVEY_INFO_FREQUENCY].type = NLA_U32; + policy[NL80211_SURVEY_INFO_NOISE].type = NLA_U8; + + nl_hdr = nlmsg_hdr(msg); + genl_hdr = (struct genlmsghdr *)nlmsg_data(nl_hdr); + genl_attr_data = genlmsg_attrdata(genl_hdr, 0); + genl_attr_len = genlmsg_attrlen(genl_hdr, 0); + + if (nla_parse(tb1, NL80211_ATTR_MAX, genl_attr_data, genl_attr_len, NULL) + != 0) { + fprintf(stderr, "ERROR parsing netlink message attributes\n"); + return NL_SKIP; + } + + if_index = nla_get_u32(tb1[NL80211_ATTR_IFINDEX]); + if (if_indextoname(if_index, if_name) == NULL) + strcpy(if_name, "ERROR"); + + if (!header) { + printf("%s:\n", if_name); + printf(" Freq Noise OnChan ChBusy ExtBusy Rxing Txing\n"); + printf("Ch MHz dBm msec msec msec msec msec\n"); + /* -- ---- - ----- -------- -------- -------- -------- -------- */ + header = 1; + } + + + if (tb1[NL80211_ATTR_SURVEY_INFO] == NULL) { + printf("no data\n"); + return NL_SKIP; + } + + if (nla_parse_nested(tb2, NL80211_SURVEY_INFO_MAX, + tb1[NL80211_ATTR_SURVEY_INFO], policy) != 0) { + printf("ERROR parsing netlink message nested attributes\n"); + return NL_SKIP; + } + + /* Concise description of what these attributes are is in linux/nl80211.h */ + + if (tb2[NL80211_SURVEY_INFO_FREQUENCY] != 0) { + uint32_t value = nla_get_u32(tb2[NL80211_SURVEY_INFO_FREQUENCY]); + int chan = freq_to_chan(value); + printf("%2d", chan); + } else { + printf("%2s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_FREQUENCY] != 0) { + uint32_t value = nla_get_u32(tb2[NL80211_SURVEY_INFO_FREQUENCY]); + printf("%5u", value); + } else { + printf("%5s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_IN_USE]) + printf("%2s", "*"); + else + printf("%2s", ""); + + if (tb2[NL80211_SURVEY_INFO_NOISE]) { + int8_t value = (int8_t)nla_get_u8(tb2[NL80211_SURVEY_INFO_NOISE]); + printf("%6d", (int)value); + } else { + printf("%6s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_CHANNEL_TIME]) { + uint64_t value = nla_get_u64(tb2[NL80211_SURVEY_INFO_CHANNEL_TIME]); + printf("%9llu", value); + } else { + printf("%9s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]) { + uint64_t value = + nla_get_u64(tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]); + printf("%9llu", value); + } else { + printf("%9s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_EXT_BUSY]) { + uint64_t value = + nla_get_u64(tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_EXT_BUSY]); + printf("%9llu", value); + } else { + printf("%9s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_RX]) { + uint64_t value = + nla_get_u64(tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_RX]); + printf("%9llu", value); + } else { + printf("%9s", ""); + } + + if (tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_TX]) { + uint64_t value = + nla_get_u64(tb2[NL80211_SURVEY_INFO_CHANNEL_TIME_TX]); + printf("%9llu", value); + } else { + printf("%9s", ""); + } + + printf("\n"); + + return NL_SKIP; +} + + +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, + void *arg) +{ + *(int *)arg = err->error; + return NL_STOP; +} + + +/* Called after all survey result messages */ +static int finish_handler(struct nl_msg *msg, void *arg) +{ + *(int *)arg = 0; + return NL_SKIP; +} + + +static int ack_handler(struct nl_msg *msg, void *arg) +{ + *(int *)arg = 0; + return NL_STOP; +} diff --git a/wifi/survey_log b/wifi/survey_log new file mode 100755 index 0000000..458b545 --- /dev/null +++ b/wifi/survey_log @@ -0,0 +1,16 @@ +#!/bin/sh + +logfile=/log/3dr-solo.log + +# This is run once from inittab when entering run level 3. +# WiFi has not quite finished initializing by then, so wait a bit. +# Running on ifup does not work; wifi is never finished scanning at that point. +sleep 10 + +echo "" >> ${logfile} + +grep ^acs_num_scans= /etc/hostapd.conf >> ${logfile} + +survey_dump >> ${logfile} + +echo "" >> ${logfile} diff --git a/wifi/wifistats.c b/wifi/wifistats.c new file mode 100644 index 0000000..143de73 --- /dev/null +++ b/wifi/wifistats.c @@ -0,0 +1,119 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" + +#define LOG_INTERVAL_US 10000 + +int getStat(int *fd, struct iwreq *iwr, int parm) +{ + if (ioctl(*fd, parm, iwr) < 0) + { + //fprintf(stderr, "Get ioctl failed\n"); + return -1; + } + + return 0; +} + +int main(void) +{ + struct iwreq iwr; + int fd; + char essid[IW_ESSID_MAX_SIZE + 1]; + char essid_save[IW_ESSID_MAX_SIZE + 1]; + struct iw_statistics stats; + char logMsg[512]; + int bitrate = 0; + int power = 0; + int signal = 0; + int noise = 0; + int missed = 0; + char timestr[24]; + FILE *logfd; + uint64_t next_us; + uint64_t now_us; + + next_us = clock_gettime_us(CLOCK_MONOTONIC) + LOG_INTERVAL_US; + + while (1) + { + + now_us = clock_gettime_us(CLOCK_MONOTONIC); + if (next_us > now_us) + usleep(next_us - now_us); + next_us += LOG_INTERVAL_US; + + clock_gettime_str_r(CLOCK_REALTIME, timestr); + + fd = socket(AF_INET, SOCK_DGRAM, 0); + strncpy(iwr.ifr_name, "wlan0", IFNAMSIZ); + + //Get the ESSID + memset(&iwr.u, 0, sizeof(struct iwreq)); + iwr.u.essid.pointer = essid; + iwr.u.essid.length = IW_ESSID_MAX_SIZE + 1; + if (!getStat(&fd, &iwr, SIOCGIWESSID)) + { + //printf("ESSID: %s\n", (char*)iwr.u.essid.pointer); + memcpy(essid_save, essid, IW_ESSID_MAX_SIZE + 1); + } + //Get the txrate + memset(&iwr.u, 0, sizeof(struct iwreq)); + if (!getStat(&fd, &iwr, SIOCGIWRATE)) + { + bitrate = iwr.u.bitrate.value; + //printf("Bitrate: %i\n", iwr.u.bitrate.value); + } + //Get the tx power + memset(&iwr.u, 0, sizeof(struct iwreq)); + if (!getStat(&fd, &iwr, SIOCGIWTXPOW)) + { + power = iwr.u.txpower.value; + //printf("TxPower: %i\n", iwr.u.txpower.value); + } + //Get the statistics + memset(&stats, 0, sizeof(stats)); + memset(&iwr.u, 0, sizeof(struct iwreq)); + iwr.u.data.pointer = &stats; + iwr.u.data.length = sizeof(stats); + if (!getStat(&fd, &iwr, SIOCGIWSTATS)) + { + if (stats.qual.updated & IW_QUAL_ALL_UPDATED + && stats.qual.updated & IW_QUAL_DBM) + { + signal = (int) stats.qual.level - 256; + noise = (int) stats.qual.noise - 256; + missed = stats.miss.beacon; + //printf("Signal level: %i dBm, noise: %i dBm\n", (int)stats.qual.level - 256, (int)stats.qual.noise - 256); + //printf("Missed: %i\n", stats.miss.beacon); + } + else + { + //printf("Signal data not ready or not in dBm\n"); + } + } + + sprintf(logMsg, + "%s, ESSID: %s, Bitrate: %i, Txpower: %i, Signal: %i, Noise: %i, MissedBeacons: %i\n", + timestr, essid_save, bitrate, power, signal, noise, + missed); + //printf("%s", logMsg); + + logfd = fopen("/log/wifistats.log", "a"); + if (logfd != NULL) + { + fputs(logMsg, logfd); + fclose(logfd); + } + + close(fd); + + } + return 0; +}