Skip to content

Commit

Permalink
AP_EXTERNAL_AHRS: Add funciton definitions to change Adnav filter set…
Browse files Browse the repository at this point in the history 
…tings
  • Loading branch information
@AN-DanielCook
AN-DanielCook committed Feb 15, 2024
1 parent 7aae480 commit d76f703
Show file tree
Hide file tree
Showing 2 changed files with 200 additions and 26 deletions.
71 changes: 48 additions & 23 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,15 +33,6 @@
#include <stdio.h>
#include <AP_BoardConfig/AP_BoardConfig.h>


#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
Expand Down Expand Up @@ -499,37 +490,37 @@ 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
}
}
};

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()) {
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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],
Expand Down Expand Up @@ -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],
Expand Down Expand Up @@ -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;
}
Expand Down
155 changes: 152 additions & 3 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Math/AP_Math.h>
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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:
Expand All @@ -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)
Expand Down Expand Up @@ -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();
};

Expand Down

0 comments on commit d76f703

Please sign in to comment.