diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp index aa534ba3fb6a93..30b5f1aeb91263 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -33,15 +33,6 @@ #include #include - -#define AN_PACKET_ID_PACKET_PERIODS 181 -#define AN_PACKET_ID_SATELLITES 30 -#define AN_PACKET_ID_RAW_GNSS 29 -#define AN_PACKET_ID_RAW_SENSORS 28 -#define AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION 25 -#define AN_PACKET_ID_SYSTEM_STATE 20 -#define AN_PACKET_ID_DEVICE_INFO 3 -#define AN_PACKET_ID_ACKNOWLEDGE 0 #define AN_TIMEOUT 5000 //ms #define AN_MAXIMUM_PACKET_PERIODS 50 #define AN_GNSS_PACKET_RATE 5 @@ -499,28 +490,28 @@ bool AP_ExternalAHRS_AdvancedNavigation::sendPacketRequest() // Update the current rate _current_rate = get_rate(); - const AN_PACKETS_PERIOD periods{ + const AN_PACKET_PERIODS periods{ permanent: true, clear_existing_packet_periods: true, periods: { AN_PERIOD{ - id: AN_PACKET_ID_SYSTEM_STATE, + id: packet_id_system_state, packet_period: (uint32_t) 1.0e3 / _current_rate }, AN_PERIOD{ - id: AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION, + id: packet_id_velocity_standard_deviation, packet_period: (uint32_t) 1.0e3 / _current_rate }, AN_PERIOD{ - id: AN_PACKET_ID_RAW_SENSORS, + id: packet_id_raw_sensors, packet_period: (uint32_t) 1.0e3 / _current_rate }, AN_PERIOD{ - id: AN_PACKET_ID_RAW_GNSS, + id: packet_id_raw_gnss, packet_period: (uint32_t) 1.0e3 / AN_GNSS_PACKET_RATE }, AN_PERIOD{ - id: AN_PACKET_ID_SATELLITES, + id: packet_id_satellites, packet_period: (uint32_t) 1.0e3 / _current_rate } } @@ -528,8 +519,8 @@ bool AP_ExternalAHRS_AdvancedNavigation::sendPacketRequest() AN_PACKET packet; // load the AN_PACKETS_PERIOD Into the payload. - packet.payload.packets_period = periods; - packet.update_checks(AN_PACKET_ID_PACKET_PERIODS, sizeof(packet.payload.packets_period)); + packet.payload.packet_periods = periods; + packet.update_checks(packet_id_packet_periods, sizeof(packet.payload.packet_periods)); // Check for space in the tx buffer if (_uart->txspace() < packet.packet_size()) { @@ -581,6 +572,40 @@ bool AP_ExternalAHRS_AdvancedNavigation::get_baro_capability(void) const return true; } +bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent) +{ + + AN_FILTER_OPTIONS options_packet; + + options_packet.permanent = permanent; + options_packet.vehicle_type = vehicle_type; + options_packet.internal_gnss_enabled = gnss_en; + options_packet.magnetometers_enabled = true; + options_packet.atmospheric_altitude_enabled = true; + options_packet.velocity_heading_enabled = true; + options_packet.reversing_detection_enabled = false; + options_packet.motion_analysis_enabled = false; + options_packet.automatic_magnetic_calibration_enabled = true; + + return set_filter_options(options_packet); +} + +bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(const AN_FILTER_OPTIONS options_packet) +{ + AN_PACKET packet; + + packet.payload.filter_options = options_packet; + packet.update_checks(packet_id_filter_options, sizeof(packet.payload.filter_options)); + + // Check for space in the tx buffer + if (_uart->txspace() < packet.packet_size()) { + return false; + } + _uart->write(packet.raw_pointer(), packet.packet_size()); + + return true; +} + void AP_ExternalAHRS_AdvancedNavigation::handle_packet() { // get current time @@ -589,13 +614,13 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet() // Update depending on received packet. switch (_msg.packet.id) { - case AN_PACKET_ID_DEVICE_INFO: { + case packet_id_device_information: { _last_device_info_pkt_ms = now; _device_id = _msg.packet.payload.device_info.device_id; _hardware_rev = _msg.packet.payload.device_info.hardware_revision; break; } - case AN_PACKET_ID_SYSTEM_STATE: { + case packet_id_system_state: { _last_state_pkt_ms = now; // Save the status _device_status.system.r = _msg.packet.payload.system_state.status.system.r; @@ -642,13 +667,13 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet() } break; } - case AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION: { + case packet_id_velocity_standard_deviation: { // save packet to be used for external gps. *_last_vel_sd = _msg.packet.payload.velocity_standard_deviation; break; } - case AN_PACKET_ID_RAW_SENSORS: { + case packet_id_raw_sensors: { AP_ExternalAHRS::ins_data_message_t ins{ accel: Vector3f{ _msg.packet.payload.raw_sensors.accelerometers[0], @@ -689,7 +714,7 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet() } break; - case AN_PACKET_ID_RAW_GNSS: { + case packet_id_raw_gnss: { // Save the standard deviations for status report _gnss_sd = Vector3f{ _msg.packet.payload.raw_gnss.llh_standard_deviation[0], @@ -769,7 +794,7 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet() } break; - case AN_PACKET_ID_SATELLITES: { + case packet_id_satellites: { // save packet to be used for external gps. *_last_satellites = _msg.packet.payload.satellites; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h index 4a6a4b540b6f09..9146f865229137 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -26,6 +26,9 @@ #define AN_PACKET_HEADER_SIZE 5 #define AN_MAXIMUM_PACKET_SIZE 255 #define AN_DECODE_BUFFER_SIZE 2*(AN_MAXIMUM_PACKET_SIZE+AN_PACKET_HEADER_SIZE) +#define AN_START_SYSTEM_PACKETS 0 +#define AN_START_STATE_PACKETS 20 +#define AN_START_CONFIGURATION_PACKETS 180 #include #include @@ -105,6 +108,122 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend private: AP_ExternalAHRS_AdvancedNavigation_Decoder _decoder; + typedef enum { + packet_id_acknowledge, + packet_id_request, + packet_id_boot_mode, + packet_id_device_information, + packet_id_restore_factory_settings, + packet_id_reset, + packet_id_6_reserved, + packet_id_file_transfer_request, + packet_id_file_transfer_acknowledge, + packet_id_file_transfer, + packet_id_serial_port_passthrough, + packet_id_ip_configuration, + packet_id_12_reserved, + packet_id_extended_device_information, + packet_id_subcomponent_information, + end_system_packets, + + packet_id_system_state = AN_START_STATE_PACKETS, + packet_id_unix_time, + packet_id_formatted_time, + packet_id_status, + packet_id_position_standard_deviation, + packet_id_velocity_standard_deviation, + packet_id_euler_orientation_standard_deviation, + packet_id_quaternion_orientation_standard_deviation, + packet_id_raw_sensors, + packet_id_raw_gnss, + packet_id_satellites, + packet_id_satellites_detailed, + packet_id_geodetic_position, + packet_id_ecef_position, + packet_id_utm_position, + packet_id_ned_velocity, + packet_id_body_velocity, + packet_id_acceleration, + packet_id_body_acceleration, + packet_id_euler_orientation, + packet_id_quaternion_orientation, + packet_id_dcm_orientation, + packet_id_angular_velocity, + packet_id_angular_acceleration, + packet_id_external_position_velocity, + packet_id_external_position, + packet_id_external_velocity, + packet_id_external_body_velocity, + packet_id_external_heading, + packet_id_running_time, + packet_id_local_magnetics, + packet_id_odometer_state, + packet_id_external_time, + packet_id_external_depth, + packet_id_geoid_height, + packet_id_rtcm_corrections, + packet_id_56_reserved, + packet_id_wind, + packet_id_heave, + packet_id_59_reserved, + packet_id_raw_satellite_data, + packet_id_raw_satellite_ephemeris, + packet_id_62_reserved, + packet_id_63_reserved, + packet_id_64_reserved, + packet_id_65_reserved, + packet_id_gnss_summary, + packet_id_external_odometer, + packet_id_external_air_data, + packet_id_gnss_receiver_information, + packet_id_raw_dvl_data, + packet_id_north_seeking_status, + packet_id_gimbal_state, + packet_id_automotive, + packet_id_74_reserved, + packet_id_external_magnetometers, + packet_id_76_reserved, + packet_id_77_reserved, + packet_id_78_reserved, + packet_id_79_reserved, + packet_id_basestation, + packet_id_81_reserved, + packet_id_82_reserved, + packet_id_zero_angular_velocity, + packet_id_extended_satellites, + packet_id_sensor_temperatures, + packet_id_system_temperature, + packet_id_87_reserved, + end_state_packets, + + packet_id_packet_timer_period = AN_START_CONFIGURATION_PACKETS, + packet_id_packet_periods, + packet_id_baud_rates, + packet_id_183_reserved, + packet_id_sensor_ranges, + packet_id_installation_alignment, + packet_id_filter_options, + packet_id_187_reserved, + packet_id_gpio_configuration, + packet_id_magnetic_calibration_values, + packet_id_magnetic_calibration_configuration, + packet_id_magnetic_calibration_status, + packet_id_odometer_configuration, + packet_id_zero_alignment, + packet_id_reference_offsets, + packet_id_gpio_output_configuration, + packet_id_dual_antenna_configuration, + packet_id_gnss_configuration, + packet_id_user_data, + packet_id_gpio_input_configuration, + packet_id_200_reserved, + packet_id_201_reserved, + packet_id_ip_dataports_configuration, + packet_id_can_configuration, + packet_id_device_name, + end_configuration_packets + } packet_id_e; + typedef enum { gnss_fix_none, gnss_fix_2d, @@ -138,7 +257,22 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend device_id_certus_mini_d, } device_id_e; - + typedef enum { + vehicle_type_unlimited, + vehicle_type_bicycle, + vehicle_type_car, + vehicle_type_hovercraft, + vehicle_type_submarine, + vehicle_type_3d_underwater, + vehicle_type_fixed_wing_plane, + vehicle_type_3d_aircraft, + vehicle_type_human, + vehicle_type_small_boat, + vehicle_type_ship, + vehicle_type_stationary, + vehicle_type_stunt_plane, + vehicle_type_race_car + } vehicle_type_e; struct PACKED AN_ACKNOWLEGE { uint8_t id_acknowledged; @@ -262,12 +396,24 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend uint32_t packet_period; }; - struct PACKED AN_PACKETS_PERIOD { + struct PACKED AN_PACKET_PERIODS { uint8_t permanent; uint8_t clear_existing_packet_periods; AN_PERIOD periods[(AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD)]; }; + struct PACKED AN_FILTER_OPTIONS { + uint8_t permanent; + uint8_t vehicle_type; + uint8_t internal_gnss_enabled; + uint8_t magnetometers_enabled; + uint8_t atmospheric_altitude_enabled; + uint8_t velocity_heading_enabled; + uint8_t reversing_detection_enabled; + uint8_t motion_analysis_enabled; + uint8_t automatic_magnetic_calibration_enabled; + }; + class PACKED AN_PACKET { public: @@ -284,7 +430,8 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend AN_RAW_SENSORS raw_sensors; AN_RAW_GNSS raw_gnss; AN_SATELLITES satellites; - AN_PACKETS_PERIOD packets_period; + AN_PACKET_PERIODS packet_periods; + AN_FILTER_OPTIONS filter_options; } payload; void update_checks(uint8_t header_id, uint8_t header_length) @@ -338,6 +485,8 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend bool sendPacketRequest(void); bool get_gnss_capability(void) const; bool get_baro_capability(void) const; + bool set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent = false); + bool set_filter_options(AN_FILTER_OPTIONS options_packet); void handle_packet(); };