Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_ExternalAHRS: added ANU CINS state estimator #24747

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
+2,339 −249
Open

AP_ExternalAHRS: added ANU CINS state estimator #24747

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
6aec684
AP_AHRS: support variances from External_AHRS
tridge Oct 14, 2024
fa6c811
AP_ExternalAHRS: support backends with get_variances()
tridge Oct 15, 2024
83817f8
waf: added AP_CINS library
tridge Aug 24, 2023
ee979d6
Copter: cope with AHRS backends with no variances
tridge Aug 11, 2024
6a1815e
AP_Math: Added LieGroups implementation
patwilt Nov 18, 2023
2c4c03a
AP_CINS: added ANU CINS state estimator
tridge Aug 24, 2023
0c1345f
AP_ExternalAHRS: added CINS backend
tridge Aug 24, 2023
71d92b4
Tools: support ExternalAHRS for Replay
tridge May 10, 2024
516069c
Tools: added feature selection for CINS
tridge Sep 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduCopter/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -480,7 +480,7 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)

// check EKF's compass, position, height and velocity variances are below failsafe threshold
if (copter.g.fs_ekf_thresh > 0.0f) {
float vel_variance, pos_variance, hgt_variance, tas_variance;
float vel_variance=0, pos_variance=0, hgt_variance=0, tas_variance=0;
Vector3f mag_variance;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance);
if (mag_variance.length() >= copter.g.fs_ekf_thresh) {
Expand Down
18 changes: 12 additions & 6 deletions Tools/Replay/LR_MsgHandler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,12 @@ void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes)
}
#undef MAP_FLAG
AP::dal().handle_message(msg, ekf2, ekf3);
if (eahrs.get_name() == nullptr) {
eahrs.init();
}
if (eahrs.get_name() != nullptr) {
eahrs.update();
}
}

void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes)
Expand Down Expand Up @@ -75,7 +81,7 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
break;
}
if (replay_force_ekf3) {
LR_MsgHandler_REV3 h{f, ekf2, ekf3};
LR_MsgHandler_REV3 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -90,7 +96,7 @@ void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes)
ekf2.setOriginLLH(loc);

if (replay_force_ekf3) {
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -100,7 +106,7 @@ void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes)
MSG_CREATE(RWA2, msgbytes);
ekf2.writeDefaultAirSpeed(msg.airspeed);
if (replay_force_ekf3) {
LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand Down Expand Up @@ -136,7 +142,7 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
}

if (replay_force_ekf2) {
LR_MsgHandler_REV2 h{f, ekf2, ekf3};
LR_MsgHandler_REV2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -150,7 +156,7 @@ void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes)
loc.alt = msg.alt;
ekf3.setOriginLLH(loc);
if (replay_force_ekf2) {
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
LR_MsgHandler_RSO2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand All @@ -160,7 +166,7 @@ void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes)
MSG_CREATE(RWA3, msgbytes);
ekf3.writeDefaultAirSpeed(msg.airspeed, msg.uncertainty);
if (replay_force_ekf2) {
LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
LR_MsgHandler_RWA2 h{f, ekf2, ekf3, eahrs};
h.process_message(msgbytes);
}
}
Expand Down
10 changes: 8 additions & 2 deletions Tools/Replay/LR_MsgHandler.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>

class LR_MsgHandler : public MsgHandler {
public:
Expand All @@ -28,15 +29,20 @@ class LR_MsgHandler_RFRH : public LR_MsgHandler
class LR_MsgHandler_EKF : public LR_MsgHandler
{
public:
LR_MsgHandler_EKF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LR_MsgHandler_EKF(struct log_Format &_f,
NavEKF2 &_ekf2,
NavEKF3 &_ekf3,
AP_ExternalAHRS &_eahrs) :
LR_MsgHandler(_f),
ekf2(_ekf2),
ekf3(_ekf3) {}
ekf3(_ekf3),
eahrs(_eahrs){}
using LR_MsgHandler::LR_MsgHandler;
virtual void process_message(uint8_t *msg) override = 0;
protected:
NavEKF2 &ekf2;
NavEKF3 &ekf3;
AP_ExternalAHRS &eahrs;
};

class LR_MsgHandler_RFRF : public LR_MsgHandler_EKF
Expand Down
33 changes: 17 additions & 16 deletions Tools/Replay/LogReader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,12 @@

extern struct user_parameter *user_parameters;

LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs) :
AP_LoggerFileReader(),
ekf2(_ekf2),
ekf3(_ekf3),
_log_structure(log_structure)
_log_structure(log_structure),
eahrs(_eahrs)
{
}

Expand Down Expand Up @@ -65,23 +66,23 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
} else if (streq(name, "RFRH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRH(formats[f.type]);
} else if (streq(name, "RFRF")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RFRN")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRN(formats[f.type]);
} else if (streq(name, "REV2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSO2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWA2")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REV3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSO3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWA3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REY3")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RISH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RISH(formats[f.type]);
} else if (streq(name, "RISI")) {
Expand Down Expand Up @@ -115,17 +116,17 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
} else if (streq(name, "RVOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RVOH(formats[f.type]);
} else if (streq(name, "ROFH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REPH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RSLL")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "REVH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RWOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3, eahrs);
} else if (streq(name, "RBOH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3);
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3, eahrs);
} else {
// debug(" No parser for (%s)\n", name);
}
Expand Down
3 changes: 2 additions & 1 deletion Tools/Replay/LogReader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
class LogReader : public AP_LoggerFileReader
{
public:
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3, AP_ExternalAHRS &_eahrs);

VehicleType::vehicle_type vehicle;

Expand All @@ -26,6 +26,7 @@ class LogReader : public AP_LoggerFileReader

NavEKF2 &ekf2;
NavEKF3 &ekf3;
AP_ExternalAHRS &eahrs;

struct LogStructure *_log_structure;
uint8_t _log_structure_count;
Expand Down
1 change: 1 addition & 0 deletions Tools/Replay/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ class Parameters {
k_param_logger,
k_param_NavEKF3,
k_param_gps,
k_param_eahrs,
};
AP_Int8 dummy;
};
Expand Down
7 changes: 7 additions & 0 deletions Tools/Replay/Replay.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,18 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
// @Group: GPS
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, "GPS", AP_GPS),

// @Group: EAHRS
// @Path: ../libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
GOBJECTPTR(eahrs, "EAHRS", AP_ExternalAHRS),

AP_VAREND
};

void ReplayVehicle::load_parameters(void)
{
eahrs = &AP::externalAHRS();

AP_Param::check_var_info();

StorageManager::erase();
Expand Down Expand Up @@ -239,6 +245,7 @@ void Replay::setup()
::printf("open(%s): %m\n", filename);
exit(1);
}
_vehicle.eahrs = &AP::externalAHRS();
}

void Replay::loop()
Expand Down
3 changes: 2 additions & 1 deletion Tools/Replay/Replay.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ class ReplayVehicle : public AP_Vehicle {

NavEKF2 ekf2;
NavEKF3 ekf3;
AP_ExternalAHRS *eahrs;

SRV_Channels servo_channels;

Expand Down Expand Up @@ -99,7 +100,7 @@ class Replay : public AP_HAL::HAL::Callbacks {
const char *filename;
ReplayVehicle &_vehicle;

LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3};
LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3, AP::externalAHRS()};

void _parse_command_line(uint8_t argc, char * const argv[]);

Expand Down
1 change: 1 addition & 0 deletions Tools/ardupilotwaf/ardupilotwaf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,7 @@
'AP_Beacon',
'AP_Arming',
'AP_RCMapper',
'AP_CINS',
]

def get_legacy_defines(sketch_name, bld):
Expand Down
9 changes: 5 additions & 4 deletions Tools/scripts/build_options.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,11 @@ def config_option(self):
Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav external AHRS', 0, "AHRS_EXT"), # noqa
Feature('AHRS', 'InertialLabs', 'AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED', 'Enable InertialLabs external AHRS', 0, "AHRS_EXT"), # noqa
Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, None),
Feature('AHRS', 'EKF3_EXTNAV', 'EK3_FEATURE_EXTERNAL_NAV', 'Enable External navigation for EKF3', 0, 'EKF3'),
Feature('AHRS', 'EKF3_WINDEST', 'EK3_FEATURE_DRAG_FUSION', 'Enable Wind estimation for EKF3', 0, 'EKF3'),
Feature('AHRS', 'EKF3_OPTFLOW', 'EK3_FEATURE_OPTFLOW_FUSION', 'Enable OpticalFlow fusion for EKF3', 0, 'EKF3,OPTICALFLOW'),
Feature('AHRS', 'BARO_WIND_COMP', 'HAL_BARO_WIND_COMP_ENABLED', 'Enable Baro wind compensation', 0, None),
Feature('AHRS', 'EKF3_EXTNAV', 'EK3_FEATURE_EXTERNAL_NAV', 'Enable External Navigation for EKF3', 0, 'EKF3'),
Feature('AHRS', 'EKF3_WINDEST', 'EK3_FEATURE_DRAG_FUSION', 'Enable Wind Estimation for EKF3', 0, 'EKF3'),
Feature('AHRS', 'EKF3_OPTFLOW', 'EK3_FEATURE_OPTFLOW_FUSION', 'Enable OpticalFlow Fusion for EKF3', 0, 'EKF3,OPTICALFLOW'),
Feature('AHRS', 'BARO_WIND_COMP', 'HAL_BARO_WIND_COMP_ENABLED', 'Enable Baro Wind Compensation', 0, None),
Feature('AHRS', 'CINS', 'AP_EXTERNAL_AHRS_CINS_ENABLED', 'Enable CINS state estimator', 0, None),

Feature('Safety', 'PARACHUTE', 'HAL_PARACHUTE_ENABLED', 'Enable Parachute', 0, None),
Feature('Safety', 'FENCE', 'AP_FENCE_ENABLED', 'Enable Geofences', 2, None),
Expand Down
2 changes: 2 additions & 0 deletions Tools/scripts/extract_features.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):

('HAL_NAVEKF3_AVAILABLE', 'NavEKF3::NavEKF3',),
('HAL_NAVEKF2_AVAILABLE', 'NavEKF2::NavEKF2',),
('AP_EXTERNAL_AHRS_CINS_ENABLED', 'CINS::init',),

('HAL_EXTERNAL_AHRS_ENABLED', r'AP_ExternalAHRS::init\b',),
('AP_EXTERNAL_AHRS_{type}_ENABLED', r'AP_ExternalAHRS_(?P<type>.*)::healthy\b',),
('HAL_INS_TEMPERATURE_CAL_ENABLE', 'AP_InertialSensor_TCal::Learn::save_calibration',),
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3164,7 +3164,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3

#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return false;
return external.get_variances(velVar, posVar, hgtVar, magVar, tasVar);
#endif
}

Expand Down
Loading
Loading