Rover: move logger object up to AP_Vehicle

This commit is contained in:
Peter Barker 2024-02-08 07:42:28 +11:00 committed by Andrew Tridgell
parent 74f2c5a881
commit aa4a805bb6
7 changed files with 24 additions and 35 deletions

View File

@ -308,22 +308,9 @@ const LogStructure Rover::log_structure[] = {
"GUIP", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
};
void Rover::log_init(void)
uint8_t Rover::get_num_log_structures() const
{
logger.Init(log_structure, ARRAY_SIZE(log_structure));
return ARRAY_SIZE(log_structure);
}
#else // LOGGING_ENABLED
// dummy functions
void Rover::Log_Write_Attitude() {}
void Rover::Log_Write_Depth() {}
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() {}
void Rover::Log_Write_Throttle() {}
void Rover::Log_Write_RC(void) {}
void Rover::Log_Write_Steering() {}
void Rover::Log_Write_Vehicle_Startup_Messages() {}
#endif // LOGGING_ENABLED

View File

@ -316,12 +316,6 @@ const AP_Param::Info Rover::var_info[] = {
// @Path: ../libraries/AP_Arming/AP_Arming.cpp
GOBJECT(arming, "ARMING_", AP_Arming),
#if HAL_LOGGING_ENABLED
// @Group: LOG
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
GOBJECT(logger, "LOG", AP_Logger),
#endif
// @Group: BATT
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT", AP_BattMonitor),
@ -944,4 +938,9 @@ void Rover::load_parameters(void)
}
#endif
// PARAMETER_CONVERSION - Added: Feb-2024 for Rover-4.6
#if HAL_LOGGING_ENABLED
AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true);
#endif
}

View File

@ -157,9 +157,6 @@ constexpr int8_t Rover::_failsafe_priorities[7];
Rover::Rover(void) :
AP_Vehicle(),
param_loader(var_info),
#if HAL_LOGGING_ENABLED
logger{g.log_bitmask},
#endif
modes(&g.mode1),
control_mode(&mode_initializing)
{

View File

@ -136,10 +136,6 @@ private:
RC_Channel *channel_pitch;
RC_Channel *channel_walking_height;
#if HAL_LOGGING_ENABLED
AP_Logger logger;
#endif
// flight modes convenience array
AP_Int8 *modes;
const uint8_t num_modes = 6;
@ -272,8 +268,6 @@ private:
} cruise_learn_t;
cruise_learn_t cruise_learn;
private:
// Rover.cpp
#if AP_SCRIPTING_ENABLED
bool set_target_location(const Location& target_loc) override;
@ -334,6 +328,14 @@ private:
// GCS_Mavlink.cpp
void send_wheel_encoder_distance(mavlink_channel_t chan);
#if HAL_LOGGING_ENABLED
// methods for AP_Vehicle:
const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
const struct LogStructure *get_log_structures() const override {
return log_structure;
}
uint8_t get_num_log_structures() const override;
// Log.cpp
void Log_Write_Attitude();
void Log_Write_Depth();
@ -345,7 +347,7 @@ private:
void Log_Write_RC(void);
void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void log_init(void);
#endif
// mode.cpp
Mode *mode_from_mode_num(enum Mode::Number num);

View File

@ -322,7 +322,9 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next
// handle guided specific initialisation and logging
_guided_mode = SubMode::WP;
send_notification = true;
#if HAL_LOGGING_ENABLED
rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f));
#endif
return true;
}
@ -338,8 +340,10 @@ void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_
_desired_speed = target_speed;
have_attitude_target = true;
#if HAL_LOGGING_ENABLED
// log new target
rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
#endif
}
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
@ -364,8 +368,10 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
_desired_speed = target_speed;
have_attitude_target = true;
#if HAL_LOGGING_ENABLED
// log new target
rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
#endif
}
// set steering and throttle (both in the range -1 to +1)

View File

@ -91,5 +91,7 @@ void Rover::update_wheel_encoder()
void Rover::read_rangefinders(void)
{
rangefinder.update();
#if HAL_LOGGING_ENABLED
Log_Write_Depth();
#endif
}

View File

@ -44,10 +44,6 @@ void Rover::init_ardupilot()
osd.init();
#endif
#if HAL_LOGGING_ENABLED
log_init();
#endif
// initialise compass
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();