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

Blimp: allow compilation with HAL_LOGGING_ENABLED false

This commit is contained in:
Peter Barker 2024-01-10 14:37:03 +11:00 committed by Andrew Tridgell
parent 6ee5ab41fd
commit 8e91c72089
17 changed files with 74 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Blimp/AP_Arming.cpp
View File

@ -304,8 +304,10 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c
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
// notify that arming will occur (we do this early to give plenty of warning) // notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true; AP_Notify::flags.armed = true;
@ -323,7 +325,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c
if (!ahrs.home_is_set()) { if (!ahrs.home_is_set()) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum(); ahrs.resetHeightDatum();
AP::logger().Write_Event(LogEvent::EKF_ALT_RESET); LOGGER_WRITE_EVENT(LogEvent::EKF_ALT_RESET);
// we have reset height, so arming height is zero // we have reset height, so arming height is zero
blimp.arming_altitude_m = 0; blimp.arming_altitude_m = 0;
@ -342,8 +344,10 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c
// finally actually arm the motors // finally actually arm the motors
blimp.motors->armed(true); blimp.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)blimp.control_mode, blimp.control_mode_reason); AP::logger().Write_Mode((uint8_t)blimp.control_mode, blimp.control_mode_reason);
#endif
// perf monitor ignores delay due to arming // perf monitor ignores delay due to arming
AP::scheduler().perf_info.ignore_this_loop(); AP::scheduler().perf_info.ignore_this_loop();
@ -391,7 +395,9 @@ bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_chec
// send disarm command to motors // send disarm command to motors
blimp.motors->armed(false); blimp.motors->armed(false);
#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);

2
Blimp/AP_State.cpp
View File

@ -10,7 +10,7 @@ void Blimp::set_auto_armed(bool b)
ap.auto_armed = b; ap.auto_armed = b;
if (b) { if (b) {
AP::logger().Write_Event(LogEvent::AUTO_ARMED); LOGGER_WRITE_EVENT(LogEvent::AUTO_ARMED);
} }
} }

19
Blimp/Blimp.cpp
View File

@ -76,7 +76,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_ServoRelayEvents, &blimp.ServoRelayEvents, update_events, 50, 75, 27), SCHED_TASK_CLASS(AP_ServoRelayEvents, &blimp.ServoRelayEvents, update_events, 50, 75, 27),
#endif #endif
SCHED_TASK_CLASS(AP_Baro, &blimp.barometer, accumulate, 50, 90, 30), SCHED_TASK_CLASS(AP_Baro, &blimp.barometer, accumulate, 50, 90, 30),
#if LOGGING_ENABLED == ENABLED #if HAL_LOGGING_ENABLED
SCHED_TASK(full_rate_logging, 50, 50, 33), SCHED_TASK(full_rate_logging, 50, 50, 33),
#endif #endif
SCHED_TASK_CLASS(AP_Notify, &blimp.notify, update, 50, 90, 36), SCHED_TASK_CLASS(AP_Notify, &blimp.notify, update, 50, 90, 36),
@ -86,13 +86,15 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
SCHED_TASK(gpsglitch_check, 10, 50, 48), SCHED_TASK(gpsglitch_check, 10, 50, 48),
SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_receive, 400, 180, 51), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_receive, 400, 180, 51),
SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_send, 400, 550, 54), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_send, 400, 550, 54),
#if LOGGING_ENABLED == ENABLED #if HAL_LOGGING_ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350, 57), SCHED_TASK(ten_hz_logging_loop, 10, 350, 57),
SCHED_TASK(twentyfive_hz_logging, 25, 110, 60), SCHED_TASK(twentyfive_hz_logging, 25, 110, 60),
SCHED_TASK_CLASS(AP_Logger, &blimp.logger, periodic_tasks, 400, 300, 63), SCHED_TASK_CLASS(AP_Logger, &blimp.logger, periodic_tasks, 400, 300, 63),
#endif #endif
SCHED_TASK_CLASS(AP_InertialSensor, &blimp.ins, periodic, 400, 50, 66), SCHED_TASK_CLASS(AP_InertialSensor, &blimp.ins, periodic, 400, 50, 66),
#if HAL_LOGGING_ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &blimp.scheduler, update_logging, 0.1, 75, 69), SCHED_TASK_CLASS(AP_Scheduler, &blimp.scheduler, update_logging, 0.1, 75, 69),
#endif
#if STATS_ENABLED == ENABLED #if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &blimp.g2.stats, update, 1, 100, 75), SCHED_TASK_CLASS(AP_Stats, &blimp.g2.stats, update, 1, 100, 75),
#endif #endif
@ -141,6 +143,7 @@ void Blimp::update_batt_compass(void)
} }
} }
#if HAL_LOGGING_ENABLED
// Full rate logging of attitude, rate and pid loops // Full rate logging of attitude, rate and pid loops
void Blimp::full_rate_logging() void Blimp::full_rate_logging()
{ {
@ -190,6 +193,7 @@ void Blimp::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 Blimp::three_hz_loop() void Blimp::three_hz_loop()
@ -201,9 +205,11 @@ void Blimp::three_hz_loop()
// one_hz_loop - runs at 1Hz // one_hz_loop - runs at 1Hz
void Blimp::one_hz_loop() void Blimp::one_hz_loop()
{ {
#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
// update assigned functions and enable auxiliary servos // update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos(); SRV_Channels::enable_aux_servos();
@ -226,6 +232,7 @@ void Blimp::read_AHRS(void)
vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)}; vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)};
vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw); vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw);
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff", AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff",
AP_HAL::micros64(), AP_HAL::micros64(),
vel_ned.x, vel_ned.x,
@ -240,6 +247,7 @@ void Blimp::read_AHRS(void)
pos_ned.y, pos_ned.y,
pos_ned.z, pos_ned.z,
blimp.ahrs.get_yaw()); blimp.ahrs.get_yaw());
#endif
} }
// read baro and log control tuning // read baro and log control tuning
@ -248,12 +256,14 @@ void Blimp::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)) {
AP::ins().write_notch_log_messages(); AP::ins().write_notch_log_messages();
#if HAL_GYROFFT_ENABLED #if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages(); gyro_fft.write_log_messages();
#endif #endif
} }
#endif
} }
//Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level. //Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level.
@ -278,7 +288,10 @@ void Blimp::rotate_NE_to_BF(Vector2f &vec)
constructor for main Blimp class constructor for main Blimp class
*/ */
Blimp::Blimp(void) Blimp::Blimp(void)
: logger(g.log_bitmask), :
#if HAL_LOGGING_ENABLED
logger(g.log_bitmask),
#endif
flight_modes(&g.flight_mode1), flight_modes(&g.flight_mode1),
control_mode(Mode::Number::MANUAL), control_mode(Mode::Number::MANUAL),
rc_throttle_control_in_filter(1.0f), rc_throttle_control_in_filter(1.0f),

4
Blimp/Blimp.h
View File

@ -114,7 +114,9 @@ private:
RC_Channel *channel_up; RC_Channel *channel_up;
RC_Channel *channel_yaw; RC_Channel *channel_yaw;
#if HAL_LOGGING_ENABLED
AP_Logger logger; AP_Logger logger;
#endif
// flight modes convenience array // flight modes convenience array
AP_Int8 *flight_modes; AP_Int8 *flight_modes;
@ -352,6 +354,7 @@ private:
// landing_gear.cpp // landing_gear.cpp
void landinggear_update(); void landinggear_update();
#if HAL_LOGGING_ENABLED
// Log.cpp // Log.cpp
void Log_Write_Performance(); void Log_Write_Performance();
void Log_Write_Attitude(); void Log_Write_Attitude();
@ -370,6 +373,7 @@ private:
void log_init(void); void log_init(void);
void Write_FINI(float right, float front, float down, float yaw); void Write_FINI(float right, float front, float down, float yaw);
void Write_FINO(float *amp, float *off); void Write_FINO(float *amp, float *off);
#endif
// mode.cpp // mode.cpp
bool set_mode(Mode::Number mode, ModeReason reason); bool set_mode(Mode::Number mode, ModeReason reason);

4
Blimp/Fins.cpp
View File

@ -80,7 +80,9 @@ void Fins::output()
yaw_out = 0; yaw_out = 0;
} }
#if HAL_LOGGING_ENABLED
blimp.Write_FINI(right_out, front_out, down_out, yaw_out); blimp.Write_FINI(right_out, front_out, down_out, yaw_out);
#endif
//Constrain after logging so as to still show when sub-optimal tuning is causing massive overshoots. //Constrain after logging so as to still show when sub-optimal tuning is causing massive overshoots.
right_out = constrain_float(right_out, -1, 1); right_out = constrain_float(right_out, -1, 1);
@ -130,7 +132,9 @@ void Fins::output()
SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX); SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX);
} }
#if HAL_LOGGING_ENABLED
blimp.Write_FINO(_amp, _off); blimp.Write_FINO(_amp, _off);
#endif
} }
void Fins::output_min() void Fins::output_min()

4
Blimp/GCS_Mavlink.cpp
View File

@ -518,7 +518,11 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS: { // MAV ID: 109 case MAVLINK_MSG_ID_RADIO_STATUS: { // MAV ID: 109
#if HAL_LOGGING_ENABLED
handle_radio_status(msg, blimp.should_log(MASK_LOG_PM)); handle_radio_status(msg, blimp.should_log(MASK_LOG_PM));
#else
handle_radio_status(msg, false);
#endif
break; break;
} }

21
Blimp/Log.cpp
View File

@ -1,6 +1,6 @@
#include "Blimp.h" #include "Blimp.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
@ -407,21 +407,4 @@ void Blimp::log_init(void)
logger.Init(log_structure, ARRAY_SIZE(log_structure)); logger.Init(log_structure, ARRAY_SIZE(log_structure));
} }
#else // LOGGING_ENABLED #endif // HAL_LOGGING_ENABLED
void Blimp::Log_Write_Performance() {}
void Blimp::Log_Write_Attitude(void) {}
void Blimp::Log_Write_PIDs(void) {}
void Blimp::Log_Write_EKF_POS() {}
void Blimp::Log_Write_Data(LogDataID id, int32_t value) {}
void Blimp::Log_Write_Data(LogDataID id, uint32_t value) {}
void Blimp::Log_Write_Data(LogDataID id, int16_t value) {}
void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {}
void Blimp::Log_Write_Data(LogDataID id, float value) {}
void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Blimp::Log_Write_Vehicle_Startup_Messages() {}
void Blimp::log_init(void) {}
#endif // LOGGING_ENABLED

6
Blimp/Loiter.cpp
View File

@ -25,10 +25,12 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
} }
scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO; scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO;
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn", AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn",
"Qffff", "Qffff",
AP_HAL::micros64(), AP_HAL::micros64(),
scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n); scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n);
#endif
float yaw_ef = blimp.ahrs.get_yaw(); float yaw_ef = blimp.ahrs.get_yaw();
Vector3f err_xyz = target_pos - blimp.pos_ned; Vector3f err_xyz = target_pos - blimp.pos_ned;
@ -120,9 +122,11 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
blimp.motors->yaw_out = act_yaw; blimp.motors->yaw_out = act_yaw;
} }
#if HAL_LOGGING_ENABLED
AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
#endif
} }
void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled) void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled)
@ -196,7 +200,9 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
blimp.motors->yaw_out = act_yaw; blimp.motors->yaw_out = act_yaw;
} }
#if HAL_LOGGING_ENABLED
AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
#endif
} }

2
Blimp/Parameters.cpp
View File

@ -341,9 +341,11 @@ const AP_Param::Info Blimp::var_info[] = {
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
GOBJECT(ahrs, "AHRS_", AP_AHRS), GOBJECT(ahrs, "AHRS_", AP_AHRS),
#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

2
Blimp/RC_Channel.cpp
View File

@ -132,7 +132,7 @@ void Blimp::save_trim()
float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f); float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f);
float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f);
ahrs.add_trim(roll_trim, pitch_trim); ahrs.add_trim(roll_trim, pitch_trim);
AP::logger().Write_Event(LogEvent::SAVE_TRIM); LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
} }

7
Blimp/config.h
View File

@ -111,13 +111,6 @@
# define AUTO_DISARMING_DELAY 10 # define AUTO_DISARMING_DELAY 10
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Default logging bitmask // Default logging bitmask
#ifndef DEFAULT_LOG_BITMASK #ifndef DEFAULT_LOG_BITMASK
# define DEFAULT_LOG_BITMASK \ # define DEFAULT_LOG_BITMASK \

16
Blimp/ekf_check.cpp
View File

@ -69,7 +69,7 @@ void Blimp::ekf_check()
// limit count from climbing too high // limit count from climbing too high
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true; ekf_check_state.bad_variance = true;
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
// send message to gcs // send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
@ -86,7 +86,7 @@ void Blimp::ekf_check()
// if compass is flagged as bad and the counter reaches zero then clear flag // if compass is flagged as bad and the counter reaches zero then clear flag
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false; ekf_check_state.bad_variance = false;
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
// clear failsafe // clear failsafe
failsafe_ekf_off_event(); failsafe_ekf_off_event();
} }
@ -145,7 +145,7 @@ void Blimp::failsafe_ekf_event()
// EKF failsafe event has occurred // EKF failsafe event has occurred
failsafe.ekf = true; failsafe.ekf = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
// does this mode require position? // does this mode require position?
if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_MANUAL)) { if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_MANUAL)) {
@ -171,7 +171,7 @@ void Blimp::failsafe_ekf_off_event(void)
} }
failsafe.ekf = false; failsafe.ekf = false;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
} }
// check for ekf yaw reset and adjust target heading, also log position reset // check for ekf yaw reset and adjust target heading, also log position reset
@ -182,13 +182,13 @@ void Blimp::check_ekf_reset()
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) { if (new_ekfYawReset_ms != ekfYawReset_ms) {
ekfYawReset_ms = new_ekfYawReset_ms; ekfYawReset_ms = new_ekfYawReset_ms;
AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
} }
// check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment
if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) {
ekf_primary_core = ahrs.get_primary_core_index(); ekf_primary_core = ahrs.get_primary_core_index();
AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core));
gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);
} }
} }
@ -232,7 +232,7 @@ void Blimp::check_vibration()
// restore ekf gains, reset timers and update user // restore ekf gains, reset timers and update user
vibration_check.high_vibes = false; vibration_check.high_vibes = false;
vibration_check.clear_ms = 0; vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
} }
} }
@ -252,7 +252,7 @@ void Blimp::check_vibration()
if (!vibration_check.high_vibes) { if (!vibration_check.high_vibes) {
// switch ekf to use resistant gains // switch ekf to use resistant gains
vibration_check.high_vibes = true; vibration_check.high_vibes = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
} }
} }

10
Blimp/events.cpp
View File

@ -14,7 +14,7 @@ bool Blimp::failsafe_option(FailsafeOption opt) const
void Blimp::failsafe_radio_on_event() void Blimp::failsafe_radio_on_event()
{ {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
// set desired action based on FS_THR_ENABLE parameter // set desired action based on FS_THR_ENABLE parameter
Failsafe_Action desired_action; Failsafe_Action desired_action;
@ -59,13 +59,13 @@ void Blimp::failsafe_radio_off_event()
{ {
// no need to do anything except log the error as resolved // no need to do anything except log the error as resolved
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
} }
void Blimp::handle_battery_failsafe(const char *type_str, const int8_t action) void Blimp::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);
Failsafe_Action desired_action = (Failsafe_Action)action; Failsafe_Action desired_action = (Failsafe_Action)action;
@ -168,10 +168,10 @@ void Blimp::gpsglitch_check()
if (ap.gps_glitching != gps_glitching) { if (ap.gps_glitching != gps_glitching) {
ap.gps_glitching = gps_glitching; ap.gps_glitching = gps_glitching;
if (gps_glitching) { if (gps_glitching) {
AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH);
gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch"); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch");
} else { } else {
AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared"); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared");
} }
} }

4
Blimp/failsafe.cpp
View File

@ -43,7 +43,7 @@ void Blimp::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;
} }
@ -59,7 +59,7 @@ void Blimp::failsafe_check()
//TODO: this may not work correctly. //TODO: this may not work correctly.
} }
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) {

8
Blimp/mode.cpp
View File

@ -78,7 +78,7 @@ bool Blimp::set_mode(Mode::Number mode, ModeReason reason)
new_flightmode->requires_GPS() && new_flightmode->requires_GPS() &&
!blimp.position_ok()) { !blimp.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;
} }
@ -89,13 +89,13 @@ bool Blimp::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(ignore_checks)) { if (!new_flightmode->init(ignore_checks)) {
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;
} }
@ -109,7 +109,9 @@ bool Blimp::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

4
Blimp/radio.cpp
View File

@ -93,7 +93,7 @@ void Blimp::read_radio()
} }
// Nobody ever talks to us. Log an error and enter failsafe. // Nobody ever talks to us. Log an error and enter failsafe.
AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
set_failsafe_radio(true); set_failsafe_radio(true);
} }
@ -155,4 +155,4 @@ void Blimp::set_throttle_zero_flag(int16_t throttle_control)
ap.throttle_zero = true; ap.throttle_zero = true;
} }
//TODO: This may not be needed //TODO: This may not be needed
} }

10
Blimp/system.cpp
View File

@ -38,7 +38,7 @@ void Blimp::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
@ -81,8 +81,10 @@ void Blimp::init_ardupilot()
barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate(); barometer.calibrate();
#if HAL_LOGGING_ENABLED
// initialise AP_Logger library // initialise AP_Logger library
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void)); logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void));
#endif
startup_INS_ground(); startup_INS_ground();
@ -224,18 +226,16 @@ void Blimp::update_auto_armed()
} }
} }
#if HAL_LOGGING_ENABLED
/* /*
should we log a message type now? should we log a message type now?
*/ */
bool Blimp::should_log(uint32_t mask) bool Blimp::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
// return MAV_TYPE corresponding to frame class // return MAV_TYPE corresponding to frame class
MAV_TYPE Blimp::get_frame_mav_type() MAV_TYPE Blimp::get_frame_mav_type()