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

Sub: correct compilation when HAL_LOGGING_ENABLED is false

This commit is contained in:
Peter Barker 2024-01-10 15:34:30 +11:00 committed by Andrew Tridgell
parent 9a853b3d4a
commit 852944a1b1
15 changed files with 86 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/AP_Arming_Sub.cpp
View File

@ -91,8 +91,10 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
return false; return false;
} }
#if HAL_LOGGING_ENABLED
// let logger know that we're armed (it may open logs e.g.) // let logger know that we're armed (it may open logs e.g.)
AP::logger().set_vehicle_armed(true); AP::logger().set_vehicle_armed(true);
#endif
// disable cpu failsafe because initialising everything takes a while // disable cpu failsafe because initialising everything takes a while
sub.mainloop_failsafe_disable(); sub.mainloop_failsafe_disable();
@ -133,8 +135,10 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
// finally actually arm the motors // finally actually arm the motors
sub.motors.armed(true); sub.motors.armed(true);
#if HAL_LOGGING_ENABLED
// log flight mode in case it was changed while vehicle was disarmed // log flight mode in case it was changed while vehicle was disarmed
AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason); AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
#endif
// reenable failsafe // reenable failsafe
sub.mainloop_failsafe_enable(); sub.mainloop_failsafe_enable();
@ -182,7 +186,9 @@ bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks
// reset the mission // reset the mission
sub.mission.reset(); sub.mission.reset();
#if HAL_LOGGING_ENABLED
AP::logger().set_vehicle_armed(false); AP::logger().set_vehicle_armed(false);
#endif
hal.util->set_soft_armed(false); hal.util->set_soft_armed(false);

12
ArduSub/ArduSub.cpp
View File

@ -89,11 +89,15 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
#if AP_CAMERA_ENABLED #if AP_CAMERA_ENABLED
SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48), SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48),
#endif #endif
#if HAL_LOGGING_ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350, 51), SCHED_TASK(ten_hz_logging_loop, 10, 350, 51),
SCHED_TASK(twentyfive_hz_logging, 25, 110, 54), SCHED_TASK(twentyfive_hz_logging, 25, 110, 54),
SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57), SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60), SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60),
#if HAL_LOGGING_ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63), SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63),
#endif
#if AP_RPM_ENABLED #if AP_RPM_ENABLED
SCHED_TASK_CLASS(AP_RPM, &sub.rpm_sensor, update, 10, 200, 66), SCHED_TASK_CLASS(AP_RPM, &sub.rpm_sensor, update, 10, 200, 66),
#endif #endif
@ -177,6 +181,7 @@ void Sub::update_batt_compass()
} }
} }
#if HAL_LOGGING_ENABLED
// ten_hz_logging_loop // ten_hz_logging_loop
// should be run at 10hz // should be run at 10hz
void Sub::ten_hz_logging_loop() void Sub::ten_hz_logging_loop()
@ -237,6 +242,7 @@ void Sub::twentyfive_hz_logging()
AP::ins().Write_IMU(); AP::ins().Write_IMU();
} }
} }
#endif // HAL_LOGGING_ENABLED
// three_hz_loop - 3.3hz loop // three_hz_loop - 3.3hz loop
void Sub::three_hz_loop() void Sub::three_hz_loop()
@ -274,9 +280,11 @@ void Sub::one_hz_loop()
AP_Notify::flags.pre_arm_gps_check = position_ok(); AP_Notify::flags.pre_arm_gps_check = position_ok();
AP_Notify::flags.flying = motors.armed(); AP_Notify::flags.flying = motors.armed();
#if HAL_LOGGING_ENABLED
if (should_log(MASK_LOG_ANY)) { if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::AP_STATE, ap.value); Log_Write_Data(LogDataID::AP_STATE, ap.value);
} }
#endif
if (!motors.armed()) { if (!motors.armed()) {
motors.update_throttle_range(); motors.update_throttle_range();
@ -285,8 +293,10 @@ void Sub::one_hz_loop()
// update assigned functions and enable auxiliary servos // update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos(); SRV_Channels::enable_aux_servos();
#if HAL_LOGGING_ENABLED
// log terrain data // log terrain data
terrain_logging(); terrain_logging();
#endif
// need to set "likely flying" when armed to allow for compass // need to set "likely flying" when armed to allow for compass
// learning to run // learning to run
@ -311,6 +321,7 @@ void Sub::update_altitude()
// read in baro altitude // read in baro altitude
read_barometer(); read_barometer();
#if HAL_LOGGING_ENABLED
if (should_log(MASK_LOG_CTUN)) { if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
AP::ins().write_notch_log_messages(); AP::ins().write_notch_log_messages();
@ -318,6 +329,7 @@ void Sub::update_altitude()
gyro_fft.write_log_messages(); gyro_fft.write_log_messages();
#endif #endif
} }
#endif // HAL_LOGGING_ENABLED
} }
bool Sub::control_check_barometer() bool Sub::control_check_barometer()

18
ArduSub/Log.cpp
View File

@ -1,6 +1,6 @@
#include "Sub.h" #include "Sub.h"
#if LOGGING_ENABLED == ENABLED #if HAL_LOGGING_ENABLED
// Code to Write and Read packets from AP_Logger log memory // Code to Write and Read packets from AP_Logger log memory
// Code to interact with the user to dump or erase logs // Code to interact with the user to dump or erase logs
@ -289,18 +289,4 @@ void Sub::log_init()
logger.Init(log_structure, ARRAY_SIZE(log_structure)); logger.Init(log_structure, ARRAY_SIZE(log_structure));
} }
#else // LOGGING_ENABLED #endif // HAL_LOGGING_ENABLED
void Sub::Log_Write_Control_Tuning() {}
void Sub::Log_Write_Attitude(void) {}
void Sub::Log_Write_Data(LogDataID id, int32_t value) {}
void Sub::Log_Write_Data(LogDataID id, uint32_t value) {}
void Sub::Log_Write_Data(LogDataID id, int16_t value) {}
void Sub::Log_Write_Data(LogDataID id, uint16_t value) {}
void Sub::Log_Write_Data(LogDataID id, float value) {}
void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Sub::Log_Write_Vehicle_Startup_Messages() {}
void Sub::log_init(void) {}
#endif // LOGGING_ENABLED

2
ArduSub/Parameters.cpp
View File

@ -561,9 +561,11 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(camera_mount, "MNT", AP_Mount), GOBJECT(camera_mount, "MNT", AP_Mount),
#endif #endif
#if HAL_LOGGING_ENABLED
// @Group: LOG // @Group: LOG
// @Path: ../libraries/AP_Logger/AP_Logger.cpp // @Path: ../libraries/AP_Logger/AP_Logger.cpp
GOBJECT(logger, "LOG", AP_Logger), GOBJECT(logger, "LOG", AP_Logger),
#endif
// @Group: BATT // @Group: BATT
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp

5
ArduSub/Sub.cpp
View File

@ -24,7 +24,10 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
constructor for main Sub class constructor for main Sub class
*/ */
Sub::Sub() Sub::Sub()
: logger(g.log_bitmask), :
#if HAL_LOGGING_ENABLED
logger(g.log_bitmask),
#endif
control_mode(Mode::Number::MANUAL), control_mode(Mode::Number::MANUAL),
motors(MAIN_LOOP_RATE), motors(MAIN_LOOP_RATE),
auto_mode(Auto_WP), auto_mode(Auto_WP),

5
ArduSub/Sub.h
View File

@ -62,6 +62,7 @@
#include <AP_JSButton/AP_JSButton.h> // Joystick/gamepad button function assignment #include <AP_JSButton/AP_JSButton.h> // Joystick/gamepad button function assignment
#include <AP_LeakDetector/AP_LeakDetector.h> // Leak detector #include <AP_LeakDetector/AP_LeakDetector.h> // Leak detector
#include <AP_Proximity/AP_Proximity.h> #include <AP_Proximity/AP_Proximity.h>
#include <AP_Rally/AP_Rally.h>
// Local modules // Local modules
#include "defines.h" #include "defines.h"
@ -145,7 +146,9 @@ private:
RC_Channel *channel_forward; RC_Channel *channel_forward;
RC_Channel *channel_lateral; RC_Channel *channel_lateral;
#if HAL_LOGGING_ENABLED
AP_Logger logger; AP_Logger logger;
#endif
AP_LeakDetector leak_detector; AP_LeakDetector leak_detector;
@ -399,6 +402,7 @@ private:
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
void rotate_body_frame_to_NE(float &x, float &y); void rotate_body_frame_to_NE(float &x, float &y);
#if HAL_LOGGING_ENABLED
void Log_Write_Control_Tuning(); void Log_Write_Control_Tuning();
void Log_Write_Attitude(); void Log_Write_Attitude();
void Log_Write_Data(LogDataID id, int32_t value); void Log_Write_Data(LogDataID id, int32_t value);
@ -408,6 +412,7 @@ private:
void Log_Write_Data(LogDataID id, float value); void Log_Write_Data(LogDataID id, float value);
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();
#endif
void load_parameters(void) override; void load_parameters(void) override;
void userhook_init(); void userhook_init();
void userhook_FastLoop(); void userhook_FastLoop();

4
ArduSub/commands_logic.cpp
View File

@ -7,10 +7,12 @@ static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCAT
// start_command - this function will be called when the ap_mission lib wishes to start a new command // start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Sub::start_command(const AP_Mission::Mission_Command& cmd) bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
{ {
#if HAL_LOGGING_ENABLED
// To-Do: logging when new commands start/end // To-Do: logging when new commands start/end
if (should_log(MASK_LOG_CMD)) { if (should_log(MASK_LOG_CMD)) {
logger.Write_Mission_Cmd(mission, cmd); logger.Write_Mission_Cmd(mission, cmd);
} }
#endif
const Location &target_loc = cmd.content.location; const Location &target_loc = cmd.content.location;
@ -347,7 +349,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
} else { } else {
// default to current altitude above origin // default to current altitude above origin
circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
} }
} }

3
ArduSub/config.h
View File

@ -186,9 +186,6 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Logging control // Logging control
// //
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Statistics // Statistics
#ifndef STATS_ENABLED #ifndef STATS_ENABLED

28
ArduSub/failsafe.cpp
View File

@ -37,7 +37,7 @@ void Sub::mainloop_failsafe_check()
failsafe_last_timestamp = tnow; failsafe_last_timestamp = tnow;
if (in_failsafe) { if (in_failsafe) {
in_failsafe = false; in_failsafe = false;
AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED);
} }
return; return;
} }
@ -51,7 +51,7 @@ void Sub::mainloop_failsafe_check()
if (motors.armed()) { if (motors.armed()) {
motors.output_min(); motors.output_min();
} }
AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED);
} }
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
@ -73,7 +73,7 @@ void Sub::failsafe_sensors_check()
// We need a depth sensor to do any sort of auto z control // We need a depth sensor to do any sort of auto z control
if (sensor_health.depth) { if (sensor_health.depth) {
if (failsafe.sensor_health) { if (failsafe.sensor_health) {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::ERROR_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::ERROR_RESOLVED);
failsafe.sensor_health = false; failsafe.sensor_health = false;
} }
return; return;
@ -86,7 +86,7 @@ void Sub::failsafe_sensors_check()
failsafe.sensor_health = true; failsafe.sensor_health = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH);
if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) { if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) {
// This should always succeed // This should always succeed
@ -137,7 +137,7 @@ void Sub::failsafe_ekf_check()
failsafe.ekf = true; failsafe.ekf = true;
AP_Notify::flags.ekf_bad = true; AP_Notify::flags.ekf_bad = true;
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) { if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
failsafe.last_ekf_warn_ms = AP_HAL::millis(); failsafe.last_ekf_warn_ms = AP_HAL::millis();
@ -152,7 +152,7 @@ void Sub::failsafe_ekf_check()
// Battery failsafe handler // Battery failsafe handler
void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
{ {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
switch((Failsafe_Action)action) { switch((Failsafe_Action)action) {
case Failsafe_Action_Surface: case Failsafe_Action_Surface:
@ -187,7 +187,7 @@ void Sub::failsafe_pilot_input_check()
failsafe.pilot_input = true; failsafe.pilot_input = true;
AP::logger().Write_Error(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
set_neutral_controls(); set_neutral_controls();
@ -270,7 +270,7 @@ void Sub::failsafe_leak_check()
// Do nothing if we are dry, or if leak failsafe action is disabled // Do nothing if we are dry, or if leak failsafe action is disabled
if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) { if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) {
if (failsafe.leak) { if (failsafe.leak) {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED);
} }
AP_Notify::flags.leak_detected = false; AP_Notify::flags.leak_detected = false;
failsafe.leak = false; failsafe.leak = false;
@ -295,7 +295,7 @@ void Sub::failsafe_leak_check()
failsafe.leak = true; failsafe.leak = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED);
// Handle failsafe action // Handle failsafe action
if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
@ -324,7 +324,7 @@ void Sub::failsafe_gcs_check()
if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) { if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe // Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) { if (failsafe.gcs) {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
} }
failsafe.gcs = false; failsafe.gcs = false;
return; return;
@ -346,7 +346,7 @@ void Sub::failsafe_gcs_check()
} }
failsafe.gcs = true; failsafe.gcs = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
// handle failsafe action // handle failsafe action
if (g.failsafe_gcs == FS_GCS_DISARM) { if (g.failsafe_gcs == FS_GCS_DISARM) {
@ -412,7 +412,7 @@ void Sub::failsafe_crash_check()
} }
failsafe.crash = true; failsafe.crash = true;
AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
// disarm motors // disarm motors
if (g.fs_crash_check == FS_CRASH_DISARM) { if (g.fs_crash_check == FS_CRASH_DISARM) {
@ -435,7 +435,7 @@ void Sub::failsafe_terrain_check()
gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered"); gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered");
failsafe_terrain_on_event(); failsafe_terrain_on_event();
} else { } else {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED);
failsafe.terrain = false; failsafe.terrain = false;
} }
} }
@ -467,7 +467,7 @@ void Sub::failsafe_terrain_set_status(bool data_ok)
void Sub::failsafe_terrain_on_event() void Sub::failsafe_terrain_on_event()
{ {
failsafe.terrain = true; failsafe.terrain = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
// If rangefinder is enabled, we can recover from this failsafe // If rangefinder is enabled, we can recover from this failsafe
if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) { if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) {

4
ArduSub/fence.cpp
View File

@ -41,10 +41,10 @@ void Sub::fence_check()
// } // }
} }
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
} else if (orig_breaches) { } else if (orig_breaches) {
// record clearing of breach // record clearing of breach
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
} }
} }

8
ArduSub/mode.cpp
View File

@ -86,7 +86,7 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason)
if (new_flightmode->requires_GPS() && if (new_flightmode->requires_GPS() &&
!sub.position_ok()) { !sub.position_ok()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false; return false;
} }
@ -96,13 +96,13 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason)
flightmode->has_manual_throttle() && flightmode->has_manual_throttle() &&
!new_flightmode->has_manual_throttle()) { !new_flightmode->has_manual_throttle()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false; return false;
} }
if (!new_flightmode->init(false)) { if (!new_flightmode->init(false)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false; return false;
} }
@ -116,7 +116,9 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason)
flightmode = new_flightmode; flightmode = new_flightmode;
control_mode = mode; control_mode = mode;
control_mode_reason = reason; control_mode_reason = reason;
#if HAL_LOGGING_ENABLED
logger.Write_Mode((uint8_t)control_mode, reason); logger.Write_Mode((uint8_t)control_mode, reason);
#endif
gcs().send_message(MSG_HEARTBEAT); gcs().send_message(MSG_HEARTBEAT);
// update notify object // update notify object

27
ArduSub/mode_guided.cpp
View File

@ -170,7 +170,7 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination)
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!sub.fence.check_destination_within_fence(dest_loc)) { if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
@ -179,8 +179,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination)
// no need to check return status because terrain data is not used // no need to check return status because terrain data is not used
sub.wp_nav.set_wp_destination(destination, false); sub.wp_nav.set_wp_destination(destination, false);
#if HAL_LOGGING_ENABLED
// log target // log target
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif
return true; return true;
} }
@ -198,7 +201,7 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc)
// reject destination outside the fence. // 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 // 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 (!sub.fence.check_destination_within_fence(dest_loc)) { if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
@ -206,13 +209,16 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc)
if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data // failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
#if HAL_LOGGING_ENABLED
// log target // log target
sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
#endif
return true; return true;
} }
@ -230,7 +236,7 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_ya
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!sub.fence.check_destination_within_fence(dest_loc)) { if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
@ -244,8 +250,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_ya
// no need to check return status because terrain data is not used // no need to check return status because terrain data is not used
sub.wp_nav.set_wp_destination(destination, false); sub.wp_nav.set_wp_destination(destination, false);
#if HAL_LOGGING_ENABLED
// log target // log target
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif
return true; return true;
} }
@ -293,7 +302,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!sub.fence.check_destination_within_fence(dest_loc)) { if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
@ -308,8 +317,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = dz; posvel_pos_target_cm.z = dz;
#if HAL_LOGGING_ENABLED
// log target // log target
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif
return true; return true;
} }
@ -325,7 +337,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!sub.fence.check_destination_within_fence(dest_loc)) { if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
@ -344,8 +356,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = dz; posvel_pos_target_cm.z = dz;
#if HAL_LOGGING_ENABLED
// log target // log target
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif
return true; return true;
} }

8
ArduSub/surface_bottom_detector.cpp
View File

@ -90,9 +90,9 @@ void Sub::set_surfaced(bool at_surface)
surface_detector_count = 0; surface_detector_count = 0;
if (ap.at_surface) { if (ap.at_surface) {
AP::logger().Write_Event(LogEvent::SURFACED); LOGGER_WRITE_EVENT(LogEvent::SURFACED);
} else { } else {
AP::logger().Write_Event(LogEvent::NOT_SURFACED); LOGGER_WRITE_EVENT(LogEvent::NOT_SURFACED);
} }
} }
@ -108,8 +108,8 @@ void Sub::set_bottomed(bool at_bottom)
bottom_detector_count = 0; bottom_detector_count = 0;
if (ap.at_bottom) { if (ap.at_bottom) {
AP::logger().Write_Event(LogEvent::BOTTOMED); LOGGER_WRITE_EVENT(LogEvent::BOTTOMED);
} else { } else {
AP::logger().Write_Event(LogEvent::NOT_BOTTOMED); LOGGER_WRITE_EVENT(LogEvent::NOT_BOTTOMED);
} }
} }

10
ArduSub/system.cpp
View File

@ -62,7 +62,7 @@ void Sub::init_ardupilot()
// setup telem slots with serial ports // setup telem slots with serial ports
gcs().setup_uarts(); gcs().setup_uarts();
#if LOGGING_ENABLED == ENABLED #if HAL_LOGGING_ENABLED
log_init(); log_init();
#endif #endif
@ -161,7 +161,7 @@ void Sub::init_ardupilot()
mission.init(); mission.init();
// initialise AP_Logger library // initialise AP_Logger library
#if LOGGING_ENABLED == ENABLED #if HAL_LOGGING_ENABLED
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void)); logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
#endif #endif
@ -264,18 +264,16 @@ bool Sub::optflow_position_ok()
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
} }
#if HAL_LOGGING_ENABLED
/* /*
should we log a message type now? should we log a message type now?
*/ */
bool Sub::should_log(uint32_t mask) bool Sub::should_log(uint32_t mask)
{ {
#if LOGGING_ENABLED == ENABLED
ap.logging_started = logger.logging_started(); ap.logging_started = logger.logging_started();
return logger.should_log(mask); return logger.should_log(mask);
#else
return false;
#endif
} }
#endif
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h> #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_Avoidance/AP_Avoidance.h> #include <AP_Avoidance/AP_Avoidance.h>

2
ArduSub/terrain.cpp
View File

@ -17,6 +17,7 @@ void Sub::terrain_update()
#endif #endif
} }
#if HAL_LOGGING_ENABLED
// log terrain data - should be called at 1hz // log terrain data - should be called at 1hz
void Sub::terrain_logging() void Sub::terrain_logging()
{ {
@ -26,4 +27,5 @@ void Sub::terrain_logging()
} }
#endif #endif
} }
#endif // HAL_LOGGING_ENABLED