From 23f262168583fa872cf4252fbbfd7fcfa443b304 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 13:52:51 +1100 Subject: [PATCH 01/12] AP_Scripting: improved dual log handling allow sync of PTHT messages between vehicle logs --- .../applets/Aerobatics/FixedWing/dual_log.py | 46 +++++++++---------- .../Aerobatics/FixedWing/plane_aerobatics.lua | 22 +++++++-- 2 files changed, 41 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_log.py b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_log.py index 4606b21acb1b8..bf2b52dc51850 100755 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_log.py +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_log.py @@ -25,19 +25,22 @@ mlog2 = mavutil.mavlink_connection(args.log2) output = open(args.logout, mode='wb') -types1 = ['ORGN','VEH','PARM','MSG','FMT','FMTU','MULT','MODE','EVT'] -types2 = ['VEH'] +types1 = ['ORGN','VEH','PARM','MSG','FMT','FMTU','MULT','MODE','EVT','PTHT'] +types2 = ['VEH','PTHT'] m1 = None m2 = None -veh_fmt = None +veh1_formats = {} -def write_VEH(m, veh_fmt): - buf = bytearray(m.get_msgbuf()) - if veh_fmt is None: +def write_message(m): + global veh1_formats + mtype = m.get_type() + if not mtype in veh1_formats: return - buf[2] = veh_fmt + veh1_fmt = veh1_formats[mtype] + buf = bytearray(m.get_msgbuf()) + buf[2] = veh1_fmt output.write(buf) m1_count = 0 @@ -49,44 +52,41 @@ def write_VEH(m, veh_fmt): if m2 is None: m2 = mlog2.recv_match(type=types2) - if m1 is not None and m1.get_type() != 'VEH': + if m1 is not None: + veh1_formats[m1.get_type()] = m1.get_msgbuf()[2] + + if m1 is not None and m1.get_type() not in types2: + # passthrough output.write(m1.get_msgbuf()) m1 = None continue - if m2 is not None and m2.get_type() != 'VEH': + if m2 is not None and m2.get_type() not in types2: continue - - if veh_fmt is None and m1 is not None: - veh_fmt = m1.get_msgbuf()[2] if m1 is None and m2 is None: break if m1 is None: - write_VEH(m2, veh_fmt) + write_message(m2) m2 = None continue if m2 is None: - write_VEH(m1, veh_fmt) + write_message(m1) m1 = None continue - if hasattr(m1,'TSec'): - # old format - t1 = m1.TSec + m1.TUSec*1.0e-6 - t2 = m2.TSec + m2.TUSec*1.0e-6 - else: - t1 = m1.GWk*7*24*60*60 + m1.GMS*0.001 - t2 = m2.GWk*7*24*60*60 + m2.GMS*0.001 + t1 = m1.GWk*7*24*60*60 + m1.GMS*0.001 + t2 = m2.GWk*7*24*60*60 + m2.GMS*0.001 + if t1 <= t2: m1_count += 1 if m1_count % args.decimate == 0: - write_VEH(m1, veh_fmt) + write_message(m1) m1 = None else: m2_count += 1 if m2_count % args.decimate == 0: - write_VEH(m2, veh_fmt) + write_message(m2) m2 = None diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index 72112bf30c5d8..2bb2c7f4e6a86 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -2130,8 +2130,10 @@ function log_pose(logname, pos, quat) math.deg(quat:get_euler_yaw())) end - -function log_position(logname, loc, quat) +--[[ + get GPS week and MS, coping with crossing a week boundary +--]] +function get_gps_times() local gps_last_fix_ms1 = gps:last_fix_time_ms(0) local gps_week = gps:time_week(0) local gps_week_ms = gps:time_week_ms(0) @@ -2144,6 +2146,11 @@ function log_position(logname, loc, quat) gps_week_ms = gps:time_week_ms(0) end gps_week_ms = gps_week_ms + (now_ms - gps_last_fix_ms2) + return gps_week, gps_week_ms +end + +function log_position(logname, loc, quat) + local gps_week, gps_week_ms = get_gps_times() logger.write(logname, 'I,GWk,GMS,Lat,Lon,Alt,R,P,Y', 'BHILLffff', @@ -2359,8 +2366,15 @@ function handle_speed_adjustment() path_var.speed_adjustment = 0.0 end - logger.write("PTHT",'SysID,RemT,LocT,TS,RemTS,PathT,RemPathT,Dt,ARPT,DE,SA','Bffffffffff', - sysid, remote_t, local_t, + local gps_week, gps_week_ms = get_gps_times() + logger.write("PTHT", + 'SysID,GWk,GMS,RemT,LocT,TS,RTS,PT,RPT,Dt,ARPT,DE,SA', + 'BHIffffffffff', + '#------------', + '-------------', + sysid, + gps_week, gps_week_ms, + remote_t, local_t, loc_timestamp, remote_timestamp, path_var.path_t, rem_patht, From 280bc3a285661140f2a2dcdb8b56506d203eb4a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 13 Feb 2024 15:15:21 +1100 Subject: [PATCH 02/12] AP_InertialSensor: fixed accel cal simple to remove unused IMUs when we change EAHRS_SENSORS to remove use of IMU from an external AHRS we need to be able to zero the accel and gyro offsets to get prearms to pass --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 9450d58447e28..5b3c4f8e7c53e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2532,7 +2532,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() } // remove existing accel offsets and scaling - for (uint8_t k=0; k Date: Sat, 2 Dec 2023 19:32:26 +1100 Subject: [PATCH 03/12] AP_ExternalAHRS: added EAHRS_LOG_RATE and common logging common logging for all EAHRS backends --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 41 +++++++++++++++++++ libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 3 ++ .../AP_ExternalAHRS_InertialLabs.cpp | 28 ++----------- .../AP_ExternalAHRS_VectorNav.cpp | 32 --------------- 4 files changed, 47 insertions(+), 57 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 9b15e6135bfe2..dd208f79da92b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -29,6 +29,7 @@ #include #include +#include extern const AP_HAL::HAL &hal; @@ -80,6 +81,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass // @User: Advanced AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF), + + // @Param: _LOG_RATE + // @DisplayName: AHRS logging rate + // @Description: Logging rate for EARHS devices + // @Units: Hz + // @User: Standard + AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10), AP_GROUPEND }; @@ -281,6 +289,39 @@ void AP_ExternalAHRS::update(void) state.have_origin = true; } } + const uint32_t now_ms = AP_HAL::millis(); + if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) { + last_log_ms = now_ms; + + // @LoggerMessage: EAHR + // @Description: External AHRS data + // @Field: TimeUS: Time since system startup + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: Lat: latitude + // @Field: Lon: longitude + // @Field: Alt: altitude AMSL + // @Field: Flg: nav status flags + + float roll, pitch, yaw; + state.quat.to_euler(roll, pitch, yaw); + nav_filter_status filterStatus {}; + get_filter_status(filterStatus); + + AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg", + "sdddnnnDUm-", + "F000000GG0-", + "QffffffLLfI", + AP_HAL::micros64(), + degrees(roll), degrees(pitch), degrees(yaw), + state.velocity.x, state.velocity.y, state.velocity.z, + state.location.lat, state.location.lng, state.location.alt*0.01, + filterStatus.value); + } } // Get model/type name diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index c904c68072d19..c00d9341fc991 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -176,6 +176,7 @@ class AP_ExternalAHRS { AP_Enum devtype; AP_Int16 rate; + AP_Int16 log_rate; AP_Int16 options; AP_Int16 sensors; @@ -190,6 +191,8 @@ class AP_ExternalAHRS { void set_default_sensors(uint16_t _sensors) { sensors.set_default(_sensors); } + + uint32_t last_log_ms; }; namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index d10ca6997c7cd..43e858a74007c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -501,28 +501,6 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @LoggerMessage: ILB1 // @Description: InertialLabs AHRS data1 // @Field: TimeUS: Time since system startup - // @Field: Roll: euler roll - // @Field: Pitch: euler pitch - // @Field: Yaw: euler yaw - // @Field: VN: velocity north - // @Field: VE: velocity east - // @Field: VD: velocity down - // @Field: Lat: latitude - // @Field: Lon: longitude - // @Field: Alt: altitude AMSL - - AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt", - "sdddnnnDUm", - "F000000GG0", - "QffffffLLf", - now_us, - degrees(roll), degrees(pitch), degrees(yaw), - state.velocity.x, state.velocity.y, state.velocity.z, - state.location.lat, state.location.lng, state.location.alt*0.01); - - // @LoggerMessage: ILB2 - // @Description: InertialLabs AHRS data2 - // @Field: TimeUS: Time since system startup // @Field: PosVarN: position variance north // @Field: PosVarE: position variance east // @Field: PosVarD: position variance down @@ -530,7 +508,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: VelVarE: velocity variance east // @Field: VelVarD: velocity variance down - AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", + AP::logger().WriteStreaming("ILB1", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", "smmmnnn", "F000000", "Qffffff", @@ -538,7 +516,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z, state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z); - // @LoggerMessage: ILB3 + // @LoggerMessage: ILB2 // @Description: InertialLabs AHRS data3 // @Field: TimeUS: Time since system startup // @Field: Stat1: unit status1 @@ -553,7 +531,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @Field: WVE: Wind velocity east // @Field: WVD: Wind velocity down - AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", + AP::logger().WriteStreaming("ILB2", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", "s-----------", "F-----------", "QHHBBBBBffff", diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 26f379868a2df..179f05fd9f1e2 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -511,38 +511,6 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) AP::ins().handle_external(ins); } - - -#if HAL_LOGGING_ENABLED - // @LoggerMessage: EAH1 - // @Description: External AHRS data - // @Field: TimeUS: Time since system startup - // @Field: Roll: euler roll - // @Field: Pitch: euler pitch - // @Field: Yaw: euler yaw - // @Field: VN: velocity north - // @Field: VE: velocity east - // @Field: VD: velocity down - // @Field: Lat: latitude - // @Field: Lon: longitude - // @Field: Alt: altitude AMSL - // @Field: UXY: uncertainty in XY position - // @Field: UV: uncertainty in velocity - // @Field: UR: uncertainty in roll - // @Field: UP: uncertainty in pitch - // @Field: UY: uncertainty in yaw - - AP::logger().WriteStreaming("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY", - "sdddnnnDUmmnddd", "F000000GG000000", - "QffffffLLffffff", - AP_HAL::micros64(), - pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0], - pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2], - int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7), - float(pkt1.positionLLA[2]), - pkt1.posU, pkt1.velU, - pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]); -#endif // HAL_LOGGING_ENABLED } /* From 98093938510c6362c81a2f8b5de198b9283da3a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 16 Feb 2024 10:36:51 +1100 Subject: [PATCH 04/12] RC_Channel: disable GPS on external AHRS with GPS_DISABLE --- libraries/RC_Channel/RC_Channel.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4fb4d153561d7..b0b4787909134 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1378,6 +1378,9 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch case AUX_FUNC::GPS_DISABLE: AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH); +#if HAL_EXTERNAL_AHRS_ENABLED + AP::externalAHRS().set_gnss_disable(ch_flag == AuxSwitchPos::HIGH); +#endif break; case AUX_FUNC::GPS_DISABLE_YAW: From 9fd3e4169ab6bbbf4d756bdd4a171a40ed97f7aa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 16 Feb 2024 10:37:34 +1100 Subject: [PATCH 05/12] AP_ExternalAHRS: added support for GPS disable and fwd flight allow backends to determine if we are in fixed wing flight and/or the GPS is disabled by the user --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 10 +++++++++- .../AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp | 6 ++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 14 +++++++++++++- 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index c00d9341fc991..e62b660815302 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -163,7 +163,12 @@ class AP_ExternalAHRS { float differential_pressure; // Pa float temperature; // degC } airspeed_data_message_t; - + + // set GNSS disable for auxillary function GPS_DISABLE + void set_gnss_disable(bool disable) { + gnss_is_disabled = disable; + } + protected: enum class OPTIONS { @@ -193,6 +198,9 @@ class AP_ExternalAHRS { } uint32_t last_log_ms; + + // true when user has disabled the GNSS + bool gnss_is_disabled; }; namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp index eaff493f5af3f..94cf8c41b7d2b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp @@ -17,6 +17,7 @@ */ #include "AP_ExternalAHRS_backend.h" +#include #if HAL_EXTERNAL_AHRS_ENABLED @@ -37,5 +38,10 @@ bool AP_ExternalAHRS_backend::option_is_set(AP_ExternalAHRS::OPTIONS option) con return frontend.option_is_set(option); } +bool AP_ExternalAHRS_backend::in_fly_forward(void) const +{ + return AP::ahrs().get_fly_forward(); +} + #endif // HAL_EXTERNAL_AHRS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index 02eb1b5319b7d..ed987a01ced3a 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -57,7 +57,19 @@ class AP_ExternalAHRS_backend { void set_default_sensors(uint16_t sensors) { frontend.set_default_sensors(sensors); } - + + /* + return true if the GNSS is disabled + */ + bool gnss_is_disabled(void) const { + return frontend.gnss_is_disabled; + } + + /* + return true when we are in fixed wing flight + */ + bool in_fly_forward(void) const; + private: AP_ExternalAHRS &frontend; }; From 9770855c9d5e600b0514adbcc684948b048f7274 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 09:20:39 +1100 Subject: [PATCH 06/12] AP_ExternalAHRS: make get_accel() and get_gyro() bool --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 16 ++++++++++++---- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 4 ++-- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index dd208f79da92b..6b91943c65c9c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -250,16 +250,24 @@ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const } } -Vector3f AP_ExternalAHRS::get_gyro(void) +bool AP_ExternalAHRS::get_gyro(Vector3f &gyro) { WITH_SEMAPHORE(state.sem); - return state.gyro; + if (!has_sensor(AvailableSensor::IMU)) { + return false; + } + gyro = state.gyro; + return true; } -Vector3f AP_ExternalAHRS::get_accel(void) +bool AP_ExternalAHRS::get_accel(Vector3f &accel) { WITH_SEMAPHORE(state.sem); - return state.accel; + if (!has_sensor(AvailableSensor::IMU)) { + return false; + } + accel = state.accel; + return true; } // send an EKF_STATUS message to GCS diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index e62b660815302..72e3161f799de 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -115,8 +115,8 @@ class AP_ExternalAHRS { bool get_speed_down(float &speedD); bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; void get_filter_status(nav_filter_status &status) const; - Vector3f get_gyro(void); - Vector3f get_accel(void); + bool get_gyro(Vector3f &gyro); + bool get_accel(Vector3f &accel); void send_status_report(class GCS_MAVLINK &link) const; // update backend From 192d9f6dbd4494bfee734468c855dcb41c2f802e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 09:22:42 +1100 Subject: [PATCH 07/12] AP_AHRS: don't use accel/gyro from ExternalAHRS unless enabled --- libraries/AP_AHRS/AP_AHRS_External.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 8cdf8ecb52bb0..435bda69b471a 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -23,7 +23,9 @@ bool AP_AHRS_External::healthy() const { void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) { Quaternion quat; - if (!AP::externalAHRS().get_quaternion(quat)) { + auto &extahrs = AP::externalAHRS(); + const AP_InertialSensor &_ins = AP::ins(); + if (!extahrs.get_quaternion(quat)) { return; } quat.rotation_matrix(results.dcm_matrix); @@ -31,9 +33,15 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad); results.gyro_drift.zero(); - results.gyro_estimate = AP::externalAHRS().get_gyro(); + if (!extahrs.get_gyro(results.gyro_estimate)) { + results.gyro_estimate = _ins.get_gyro(); + } + + Vector3f accel; + if (!extahrs.get_accel(accel)) { + accel = _ins.get_accel(); + } - const Vector3f accel = AP::externalAHRS().get_accel(); const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel; results.accel_ef = accel_ef; From 0aee2a436ce27267d86264a03cb2ebb220ffcb17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 09:38:55 +1100 Subject: [PATCH 08/12] AP_ExternalAHRS: check for origin in pre-arm check --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 6b91943c65c9c..cbcc394fb042f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -236,6 +236,10 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) return false; } + if (!state.have_origin) { + hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin"); + return false; + } return backend->pre_arm_check(failure_msg, failure_msg_len); } From 0fe9027b236452e1a5dc7d305694edd6fb3449e8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 17 Feb 2024 10:08:03 +1100 Subject: [PATCH 09/12] GCS_MAVLink: handle MAV_CMD_DO_SET_SAFETY_SWITCH_STATE --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 22 ++++++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 95cc4237fe2ee..736cadfa053a9 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -595,6 +595,8 @@ class GCS_MAVLINK void deadlock_sem(void); #endif + MAV_RESULT handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + // reset a message interval via mavlink: MAV_RESULT handle_command_set_message_interval(const mavlink_command_int_t &packet); MAV_RESULT handle_command_get_message_interval(const mavlink_command_int_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 4889e90b5d4d0..a907c003f3d6d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5133,6 +5133,25 @@ MAV_RESULT GCS_MAVLINK::handle_command_storage_format(const mavlink_command_int_ } #endif +MAV_RESULT GCS_MAVLINK::handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + switch ((SAFETY_SWITCH_STATE)packet.param1) { + case SAFETY_SWITCH_STATE_DANGEROUS: + // turn safety off (pwm outputs flow to the motors) + hal.rcout->force_safety_off(); + return MAV_RESULT_ACCEPTED; + case SAFETY_SWITCH_STATE_SAFE: + // turn safety on (no pwm outputs to the motors) + if (hal.rcout->force_safety_on()) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + default: + return MAV_RESULT_DENIED; + } +} + + MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -5290,6 +5309,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: return handle_preflight_reboot(packet, msg); + case MAV_CMD_DO_SET_SAFETY_SWITCH_STATE: + return handle_do_set_safety_switch_state(packet, msg); + #if AP_MAVLINK_SERVO_RELAY_ENABLED case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_REPEAT_SERVO: From ae504ba564e2acfdf1b47582a0ae3a9bb90626be Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 17 Feb 2024 10:08:27 +1100 Subject: [PATCH 10/12] mavlink: reference new MAV_CMD_DO_SET_SAFETY_SWITCH_STATE command --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 130a836efbfef..76a2c501fcbac 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 130a836efbfef0eb3287a92cd5e187de7facdce2 +Subproject commit 76a2c501fcbaccd833c8cb7eaa8389e26a9667a0 From 9aa6193568791d7c71b56bb76439fe4c7983e4bd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 17:19:27 +1100 Subject: [PATCH 11/12] autotest: test new safety switch command --- Tools/autotest/arducopter.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d89ffa43f5ea6..043323a1f6d45 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10281,6 +10281,12 @@ def SafetySwitch(self): self.change_mode('LAND') self.wait_disarmed() + # test turning safty on/off using explicit MAVLink command: + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_SAFE) + self.assert_prearm_failure("safety switch") + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_DANGEROUS) + self.wait_ready_to_arm() + def GuidedYawRate(self): '''ensuer guided yaw rate is not affected by rate of sewt-attitude messages''' self.takeoff(30, mode='GUIDED') From 079ffb4a400714689792e54babca6d45938bde1d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 27 Feb 2024 10:51:19 +1100 Subject: [PATCH 12/12] AP_Param: add comments around G2 parameter conversion --- libraries/AP_Param/AP_Param.cpp | 2 ++ libraries/AP_Param/AP_Param.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 826c308a467fb..0beaeb860a1a2 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2072,6 +2072,8 @@ void AP_Param::convert_class(uint16_t param_key, void *object_pointer, flush(); } +// convert an object which was stored in a vehicle's G2 into a new +// object in AP_Vehicle.cpp: void AP_Param::convert_g2_objects(const void *g2, const G2ObjectConversion g2_conversions[], uint8_t num_conversions) { // Find G2's Top Level Key diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 2aeb19a8a28e6..1a9284a71e74e 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -472,6 +472,8 @@ class AP_Param // convert old vehicle parameters to new object parameters with scaling - assumes we use the same scaling factor for all values in the table static void convert_old_parameters_scaled(const ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags); + // convert an object which was stored in a vehicle's G2 into a new + // object in AP_Vehicle.cpp: struct G2ObjectConversion { void *object_pointer; const struct AP_Param::GroupInfo *var_info;