Rover: move logger object up to AP_Vehicle
This commit is contained in:
parent
74f2c5a881
commit
aa4a805bb6
@ -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
|
||||
|
@ -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
|
||||
|
||||
}
|
||||
|
@ -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)
|
||||
{
|
||||
|
@ -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);
|
||||
|
@ -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)
|
||||
|
@ -91,5 +91,7 @@ void Rover::update_wheel_encoder()
|
||||
void Rover::read_rangefinders(void)
|
||||
{
|
||||
rangefinder.update();
|
||||
#if HAL_LOGGING_ENABLED
|
||||
Log_Write_Depth();
|
||||
#endif
|
||||
}
|
||||
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user