Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: move Log_Write_Error into library

This commit is contained in:
Peter Barker 2019-03-25 12:06:24 +11:00 committed by Peter Barker
parent d2a683a758
commit 4d4a63cc33
7 changed files with 15 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
APMrover2/Log.cpp
View File

@ -83,25 +83,6 @@ void Rover::Log_Write_Depth()
(double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f));
}
struct PACKED log_Error {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t sub_system;
uint8_t error_code;
};
// Write an error packet
void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
{
struct log_Error pkt = {
LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
time_us : AP_HAL::micros64(),
sub_system : sub_system,
error_code : error_code,
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
// guided mode logging
struct PACKED log_GuidedTarget {
LOG_PACKET_HEADER;
@ -335,8 +316,6 @@ const LogStructure Rover::log_structure[] = {
"STER", "Qhfffff", "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" },
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" },
};
void Rover::log_init(void)
@ -350,7 +329,6 @@ void Rover::log_init(void)
void Rover::Log_Write_Arm_Disarm() {}
void Rover::Log_Write_Attitude() {}
void Rover::Log_Write_Depth() {}
void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Rover::Log_Write_Nav_Tuning() {}
void Rover::Log_Write_Sail() {}

1
APMrover2/Rover.h
View File

@ -423,7 +423,6 @@ private:
void Log_Write_Arm_Disarm();
void Log_Write_Attitude();
void Log_Write_Depth();
void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Nav_Tuning();
void Log_Write_Sail();

3
APMrover2/crash_check.cpp
View File

@ -46,7 +46,8 @@ void Rover::crash_check()
if (crashed) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK,
LogErrorCode::CRASH_CHECK_CRASH);
if (is_balancebot()) {
// send message to gcs

18
APMrover2/defines.h
View File

@ -29,7 +29,6 @@
#define LOG_ARM_DISARM_MSG 0x08
#define LOG_STEERING_MSG 0x0D
#define LOG_GUIDEDTARGET_MSG 0x0E
#define LOG_ERROR_MSG 0x13
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
@ -67,23 +66,6 @@
#define MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE (1<<6)
#define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE (1<<7)
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_EKFCHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
// EKF check definitions
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// subsystem specific error codes -- ekf failsafe
#define ERROR_CODE_FAILSAFE_RESOLVED 0
#define ERROR_CODE_FAILSAFE_OCCURRED 1
// radio failsafe enum (FS_THR_ENABLE parameter)
enum fs_thr_enable {
FS_THR_DISABLED = 0,

12
APMrover2/ekf_check.cpp
View File

@ -53,7 +53,8 @@ void Rover::ekf_check()
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK,
LogErrorCode::EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
@ -71,7 +72,8 @@ void Rover::ekf_check()
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK,
LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
@ -150,7 +152,8 @@ void Rover::failsafe_ekf_event()
// EKF failsafe event has occurred
failsafe.ekf = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV,
LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF failsafe!");
// does this mode require position?
@ -180,6 +183,7 @@ void Rover::failsafe_ekf_off_event(void)
// clear flag and log recovery
failsafe.ekf = false;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV,
LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF failsafe cleared");
}

5
APMrover2/fence.cpp
View File

@ -29,10 +29,11 @@ void Rover::fence_check()
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
} else if (orig_breaches) {
// record clearing of breach
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE,
LogErrorCode::ERROR_RESOLVED);
}
}

3
APMrover2/system.cpp
View File

@ -240,7 +240,8 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
Mode &old_mode = *control_mode;
if (!new_mode.enter()) {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE, new_mode.mode_number());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE,
LogErrorCode(new_mode.mode_number()));
gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
return false;
}