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

Sub: move Log_Write_Error into library

This commit is contained in:
Peter Barker 2019-03-25 12:53:26 +11:00 committed by Peter Barker
parent 0d0dfd49bd
commit 706a75ad03
10 changed files with 25 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduSub/Log.cpp
View File

@ -202,38 +202,19 @@ void Sub::Log_Write_Data(uint8_t id, float value)
}
}
struct PACKED log_Error {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t sub_system;
uint8_t error_code;
};
// Write an error packet
void Sub::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.WriteCriticalBlock(&pkt, sizeof(pkt));
}
// logs when baro or compass becomes unhealthy
void Sub::Log_Sensor_Health()
{
// check baro
if (sensor_health.baro != barometer.healthy()) {
sensor_health.baro = barometer.healthy();
Log_Write_Error(ERROR_SUBSYSTEM_BARO, (sensor_health.baro ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
AP::logger().Write_Error(LogErrorSubsystem::BARO, (sensor_health.baro ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY));
}
// check compass
if (sensor_health.compass != compass.healthy()) {
sensor_health.compass = compass.healthy();
Log_Write_Error(ERROR_SUBSYSTEM_COMPASS, (sensor_health.compass ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
AP::logger().Write_Error(LogErrorSubsystem::COMPASS, (sensor_health.compass ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY));
}
}
@ -287,8 +268,6 @@ const struct LogStructure Sub::log_structure[] = {
"DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" },
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
};
@ -319,7 +298,6 @@ void Sub::Log_Write_Data(uint8_t id, uint32_t value) {}
void Sub::Log_Write_Data(uint8_t id, int16_t value) {}
void Sub::Log_Write_Data(uint8_t id, uint16_t value) {}
void Sub::Log_Write_Data(uint8_t id, float value) {}
void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Sub::Log_Sensor_Health() {}
void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Sub::Log_Write_Vehicle_Startup_Messages() {}

1
ArduSub/Sub.h
View File

@ -492,7 +492,6 @@ private:
void Log_Write_Data(uint8_t id, int16_t value);
void Log_Write_Data(uint8_t id, uint16_t value);
void Log_Write_Data(uint8_t id, float value);
void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
void Log_Sensor_Health();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Vehicle_Startup_Messages();

2
ArduSub/commands_logic.cpp
View File

@ -360,7 +360,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
} else {
// default to current altitude above origin
circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
}
}

2
ArduSub/control_auto.cpp
View File

@ -259,7 +259,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
}
circle_nav.set_center(circle_center_neu);

8
ArduSub/control_guided.cpp
View File

@ -149,7 +149,7 @@ bool Sub::guided_set_destination(const Vector3f& destination)
// reject destination if outside the fence
const Location dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
@ -177,7 +177,7 @@ bool Sub::guided_set_destination(const Location& dest_loc)
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
@ -185,7 +185,7 @@ bool Sub::guided_set_destination(const Location& dest_loc)
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
@ -221,7 +221,7 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
// reject destination if outside the fence
const Location dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}

53
ArduSub/defines.h
View File

@ -105,7 +105,6 @@ enum LoggingParameters {
TYPE_AIRSTART_MSG,
TYPE_GROUNDSTART_MSG,
LOG_CONTROL_TUNING_MSG,
LOG_ERROR_MSG,
LOG_DATA_INT16_MSG,
LOG_DATA_UINT16_MSG,
LOG_DATA_INT32_MSG,
@ -136,58 +135,6 @@ enum LoggingParameters {
#define MASK_LOG_IMU_RAW (1UL<<19)
#define MASK_LOG_ANY 0xFFFF
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_MAIN 1
#define ERROR_SUBSYSTEM_INPUT 2
#define ERROR_SUBSYSTEM_COMPASS 3
#define ERROR_SUBSYSTEM_OPTFLOW 4
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 // not used
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11 // not used
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_EKFCHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
#define ERROR_SUBSYSTEM_BARO 18
#define ERROR_SUBSYSTEM_CPU 19
#define ERROR_SUBSYSTEM_TERRAIN 21
#define ERROR_SUBSYSTEM_NAVIGATION 22
#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23
#define ERROR_SUBSYSTEM_FAILSAFE_LEAK 24
#define ERROR_SUBSYSTEM_FAILSAFE_SENSORS 25
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
#define ERROR_CODE_UNHEALTHY 4
// subsystem specific error codes -- radio
#define ERROR_CODE_RADIO_LATE_FRAME 2
// subsystem specific error codes -- failsafe_thr, batt, gps
#define ERROR_CODE_FAILSAFE_RESOLVED 0
#define ERROR_CODE_FAILSAFE_OCCURRED 1
// subsystem specific error codes -- main
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
#define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2
// subsystem specific error codes -- terrain
#define ERROR_CODE_MISSING_TERRAIN_DATA 2
// subsystem specific error codes -- navigation
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
#define ERROR_CODE_RESTARTED_RTL 3
#define ERROR_CODE_FAILED_CIRCLE_INIT 4
#define ERROR_CODE_DEST_OUTSIDE_FENCE 5
// EKF check definitions
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BAD_DEPTH 0
// GCS failsafe
#ifndef FS_GCS
# define FS_GCS DISABLED

26
ArduSub/failsafe.cpp
View File

@ -37,7 +37,7 @@ void Sub::mainloop_failsafe_check()
failsafe_last_timestamp = tnow;
if (in_failsafe) {
in_failsafe = false;
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED);
}
return;
}
@ -52,7 +52,7 @@ void Sub::mainloop_failsafe_check()
motors.output_min();
}
// log an error
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED);
}
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
@ -84,7 +84,7 @@ void Sub::failsafe_sensors_check()
failsafe.sensor_health = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH);
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
// This should always succeed
@ -136,7 +136,7 @@ void Sub::failsafe_ekf_check()
failsafe.ekf = true;
AP_Notify::flags.ekf_bad = true;
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
failsafe.last_ekf_warn_ms = AP_HAL::millis();
@ -151,7 +151,7 @@ void Sub::failsafe_ekf_check()
// Battery failsafe handler
void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
{
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
switch((Failsafe_Action)action) {
case Failsafe_Action_Surface:
@ -186,7 +186,7 @@ void Sub::failsafe_pilot_input_check()
failsafe.pilot_input = true;
Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
set_neutral_controls();
@ -269,7 +269,7 @@ void Sub::failsafe_leak_check()
// Do nothing if we are dry, or if leak failsafe action is disabled
if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) {
if (failsafe.leak) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED);
}
AP_Notify::flags.leak_detected = false;
failsafe.leak = false;
@ -294,7 +294,7 @@ void Sub::failsafe_leak_check()
failsafe.leak = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED);
// Handle failsafe action
if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
@ -317,7 +317,7 @@ void Sub::failsafe_gcs_check()
if (tnow - failsafe.last_heartbeat_ms < FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
}
failsafe.gcs = false;
return;
@ -340,7 +340,7 @@ void Sub::failsafe_gcs_check()
// update state, log to dataflash
failsafe.gcs = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
// handle failsafe action
if (g.failsafe_gcs == FS_GCS_DISARM) {
@ -407,7 +407,7 @@ void Sub::failsafe_crash_check()
failsafe.crash = true;
// 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);
// disarm motors
if (g.fs_crash_check == FS_CRASH_DISARM) {
@ -430,7 +430,7 @@ void Sub::failsafe_terrain_check()
gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered");
failsafe_terrain_on_event();
} else {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED);
failsafe.terrain = false;
}
}
@ -462,7 +462,7 @@ void Sub::failsafe_terrain_set_status(bool data_ok)
void Sub::failsafe_terrain_on_event()
{
failsafe.terrain = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
// If rangefinder is enabled, we can recover from this failsafe
if (!rangefinder_state.enabled || !auto_terrain_recover_start()) {

4
ArduSub/fence.cpp
View File

@ -42,10 +42,10 @@ void Sub::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);
}
}

2
ArduSub/flight_mode.cpp
View File

@ -87,7 +87,7 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
#endif
} else {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
}
// return success or failure

2
ArduSub/sensors.cpp
View File

@ -87,7 +87,7 @@ void Sub::init_compass()
if (!compass.read()) {
// make sure we don't pass a broken compass to DCM
hal.console->println("COMPASS INIT ERROR");
Log_Write_Error(ERROR_SUBSYSTEM_COMPASS,ERROR_CODE_FAILED_TO_INITIALISE);
AP::logger().Write_Error(LogErrorSubsystem::COMPASS,LogErrorCode::FAILED_TO_INITIALISE);
return;
}
ahrs.set_compass(&compass);