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

AP_ExternalAHRS: added ANU CINS state estimator #24747

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
+2,217 −39
Open

AP_ExternalAHRS: added ANU CINS state estimator #24747

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
456b63f
waf: added AP_CINS library
tridge Aug 24, 2023
26f5560
Copter: cope with AHRS backends with no variances
tridge Aug 11, 2024
950ee0e
AP_Math: Added LieGroups implementation
patwilt Nov 18, 2023
2c50156
AP_CINS: added ANU CINS state estimator
tridge Aug 24, 2023
837ca65
AP_ExternalAHRS: added CINS backend
tridge Aug 24, 2023
7c64f82
Tools: support ExternalAHRS for Replay
tridge May 10, 2024
d45dc8d
Tools: added feature selection for CINS
tridge Sep 11, 2024
610cc28
Tools: make CINS depend on AHRS_EXT
tridge Oct 31, 2024
efc5335
waf: added 1 level recursive depedencies
tridge Oct 31, 2024
6ae6607
Tools: don't build replay on low-end STM32 boards
tridge Oct 31, 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
11 changes: 8 additions & 3 deletions Tools/Replay/wscript
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,14 @@ def configure(cfg):
cfg.env.HAL_GCS_ENABLED = 0

def build(bld):
if isinstance(bld.get_board(), boards.chibios) and bld.env['WITH_FATFS'] != '1':
# we need a filesystem for replay
return
print(bld.env)
if isinstance(bld.get_board(), boards.chibios):
if bld.env['WITH_FATFS'] != '1':
# we need a filesystem for replay
return
if not bld.env['APJ_BOARD_TYPE'].startswith('STM32H7'):
# need an H7
return

vehicle = bld.path.name

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
6 changes: 6 additions & 0 deletions Tools/ardupilotwaf/boards.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,12 @@ def srcpath(path):
disable_option = "disable_" + enable_option[len("enable-"):]
if getattr(cfg.options, enable_option, False):
env.CXXFLAGS += ['-D%s=1' % opt.define]
if opt.dependency:
for d in opt.dependency.split(','):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lift method from build_options.py?

for opt2 in build_options.BUILD_OPTIONS:
if opt2.label == d:
cfg.msg("Enabled %s" % opt2.label, 'yes', color='GREEN')
env.CXXFLAGS += ['-D%s=1' % opt2.define]
cfg.msg("Enabled %s" % opt.label, 'yes', color='GREEN')
elif getattr(cfg.options, disable_option, False):
env.CXXFLAGS += ['-D%s=0' % opt.define]
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'),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

case problems

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, 'AHRS_EXT'),

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
Loading
Loading