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

Rover: Use common variables

This commit is contained in:
muramura 2024-05-03 19:02:37 +09:00 committed by Andrew Tridgell
parent 5107cb495a
commit f7cc7eea90
7 changed files with 32 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Rover/Log.cpp
View File

@ -24,7 +24,7 @@ void Rover::Log_Write_Attitude()
} }
// log heel to sail control for sailboats // log heel to sail control for sailboats
if (rover.g2.sailboat.sail_enabled()) { if (g2.sailboat.sail_enabled()) {
logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info()); logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
} }
} }
@ -142,13 +142,13 @@ void Rover::Log_Write_Nav_Tuning()
void Rover::Log_Write_Sail() void Rover::Log_Write_Sail()
{ {
// only log sail if present // only log sail if present
if (!rover.g2.sailboat.sail_enabled()) { if (!g2.sailboat.sail_enabled()) {
return; return;
} }
float wind_dir_tack = logger.quiet_nanf(); float wind_dir_tack = logger.quiet_nanf();
uint8_t current_tack = 0; uint8_t current_tack = 0;
if (rover.g2.windvane.enabled()) { if (g2.windvane.enabled()) {
wind_dir_tack = degrees(g2.windvane.get_tack_threshold_wind_dir_rad()); wind_dir_tack = degrees(g2.windvane.get_tack_threshold_wind_dir_rad());
current_tack = uint8_t(g2.windvane.get_current_tack()); current_tack = uint8_t(g2.windvane.get_current_tack());
} }

14
Rover/mode.cpp
View File

@ -10,7 +10,7 @@ Mode::Mode() :
channel_roll(rover.channel_roll), channel_roll(rover.channel_roll),
channel_pitch(rover.channel_pitch), channel_pitch(rover.channel_pitch),
channel_walking_height(rover.channel_walking_height), channel_walking_height(rover.channel_walking_height),
attitude_control(rover.g2.attitude_control) attitude_control(g2.attitude_control)
{ } { }
void Mode::exit() void Mode::exit()
@ -47,7 +47,7 @@ bool Mode::enter()
set_reversed(false); set_reversed(false);
// clear sailboat tacking flags // clear sailboat tacking flags
rover.g2.sailboat.clear_tack(); g2.sailboat.clear_tack();
} }
return ret; return ret;
@ -66,7 +66,7 @@ void Mode::get_pilot_input(float &steering_out, float &throttle_out) const
} }
// apply RC skid steer mixing // apply RC skid steer mixing
switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get()) switch ((enum pilot_steer_type_t)g.pilot_steer_type.get())
{ {
case PILOT_STEER_TYPE_DEFAULT: case PILOT_STEER_TYPE_DEFAULT:
case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING: case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING:
@ -166,7 +166,7 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_
float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f); float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f);
// handle two paddle input // handle two paddle input
if ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) { if ((enum pilot_steer_type_t)g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) {
const float left_paddle = desired_steering; const float left_paddle = desired_steering;
const float right_paddle = desired_throttle; const float right_paddle = desired_throttle;
desired_steering = (left_paddle - right_paddle) * 0.5f; desired_steering = (left_paddle - right_paddle) * 0.5f;
@ -279,7 +279,7 @@ void Mode::handle_tack_request()
{ {
// autopilot modes handle tacking // autopilot modes handle tacking
if (is_autopilot_mode()) { if (is_autopilot_mode()) {
rover.g2.sailboat.handle_tack_request_auto(); g2.sailboat.handle_tack_request_auto();
} }
} }
@ -304,9 +304,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
// call throttle controller and convert output to -100 to +100 range // call throttle controller and convert output to -100 to +100 range
float throttle_out = 0.0f; float throttle_out = 0.0f;
if (rover.g2.sailboat.sail_enabled()) { if (g2.sailboat.sail_enabled()) {
// sailboats use special throttle and mainsail controller // sailboats use special throttle and mainsail controller
rover.g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out); g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out);
} else { } else {
// call speed or stop controller // call speed or stop controller
if (is_zero(target_speed) && !rover.is_balancebot()) { if (is_zero(target_speed) && !rover.is_balancebot()) {

8
Rover/mode_acro.cpp
View File

@ -28,12 +28,12 @@ void ModeAcro::update()
// handle sailboats // handle sailboats
if (!is_zero(desired_steering)) { if (!is_zero(desired_steering)) {
// steering input return control to user // steering input return control to user
rover.g2.sailboat.clear_tack(); g2.sailboat.clear_tack();
} }
if (rover.g2.sailboat.tacking()) { if (g2.sailboat.tacking()) {
// call heading controller during tacking // call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.g2.sailboat.get_tack_heading_rad(), steering_out = attitude_control.get_steering_out_heading(g2.sailboat.get_tack_heading_rad(),
g2.wp_nav.get_pivot_rate(), g2.wp_nav.get_pivot_rate(),
g2.motors.limit.steer_left, g2.motors.limit.steer_left,
g2.motors.limit.steer_right, g2.motors.limit.steer_right,
@ -60,5 +60,5 @@ bool ModeAcro::requires_velocity() const
// sailboats in acro mode support user manually initiating tacking from transmitter // sailboats in acro mode support user manually initiating tacking from transmitter
void ModeAcro::handle_tack_request() void ModeAcro::handle_tack_request()
{ {
rover.g2.sailboat.handle_tack_request_acro(); g2.sailboat.handle_tack_request_acro();
} }

20
Rover/mode_auto.cpp
View File

@ -141,7 +141,7 @@ void ModeAuto::update()
break; break;
case SubMode::Circle: case SubMode::Circle:
rover.g2.mode_circle.update(); g2.mode_circle.update();
break; break;
} }
} }
@ -173,7 +173,7 @@ float ModeAuto::wp_bearing() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.wp_bearing(); return rover.mode_guided.wp_bearing();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.wp_bearing(); return g2.mode_circle.wp_bearing();
} }
// this line should never be reached // this line should never be reached
@ -197,7 +197,7 @@ float ModeAuto::nav_bearing() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.nav_bearing(); return rover.mode_guided.nav_bearing();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.nav_bearing(); return g2.mode_circle.nav_bearing();
} }
// this line should never be reached // this line should never be reached
@ -221,7 +221,7 @@ float ModeAuto::crosstrack_error() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.crosstrack_error(); return rover.mode_guided.crosstrack_error();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.crosstrack_error(); return g2.mode_circle.crosstrack_error();
} }
// this line should never be reached // this line should never be reached
@ -245,7 +245,7 @@ float ModeAuto::get_desired_lat_accel() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_lat_accel(); return rover.mode_guided.get_desired_lat_accel();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_desired_lat_accel(); return g2.mode_circle.get_desired_lat_accel();
} }
// this line should never be reached // this line should never be reached
@ -270,7 +270,7 @@ float ModeAuto::get_distance_to_destination() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_distance_to_destination(); return rover.mode_guided.get_distance_to_destination();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_distance_to_destination(); return g2.mode_circle.get_distance_to_destination();
} }
// this line should never be reached // this line should never be reached
@ -299,7 +299,7 @@ bool ModeAuto::get_desired_location(Location& destination) const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_location(destination); return rover.mode_guided.get_desired_location(destination);
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_desired_location(destination); return g2.mode_circle.get_desired_location(destination);
} }
// we should never reach here but just in case // we should never reach here but just in case
@ -341,7 +341,7 @@ bool ModeAuto::reached_destination() const
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.reached_destination(); return rover.mode_guided.reached_destination();
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.reached_destination(); return g2.mode_circle.reached_destination();
} }
// we should never reach here but just in case, return true to allow missions to continue // we should never reach here but just in case, return true to allow missions to continue
@ -366,7 +366,7 @@ bool ModeAuto::set_desired_speed(float speed)
case SubMode::NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.set_desired_speed(speed); return rover.mode_guided.set_desired_speed(speed);
case SubMode::Circle: case SubMode::Circle:
return rover.g2.mode_circle.set_desired_speed(speed); return g2.mode_circle.set_desired_speed(speed);
} }
return false; return false;
} }
@ -902,7 +902,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// if a location target was set, return true once vehicle is close // if a location target was set, return true once vehicle is close
if (guided_target.valid) { if (guided_target.valid) {
if (rover.current_loc.get_distance(guided_target.loc) <= rover.g2.wp_nav.get_radius()) { if (rover.current_loc.get_distance(guided_target.loc) <= g2.wp_nav.get_radius()) {
return true; return true;
} }
} }

8
Rover/mode_circle.cpp
View File

@ -149,7 +149,7 @@ void ModeCircle::update()
_distance_to_destination = center_to_veh.length(); _distance_to_destination = center_to_veh.length();
dist_to_edge_m = fabsf(_distance_to_destination - config.radius); dist_to_edge_m = fabsf(_distance_to_destination - config.radius);
if (!reached_edge) { if (!reached_edge) {
const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST); const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
reached_edge = dist_to_edge_m <= dist_thresh_m; reached_edge = dist_to_edge_m <= dist_thresh_m;
} }
@ -266,8 +266,8 @@ void ModeCircle::check_config_speed()
void ModeCircle::check_config_radius() void ModeCircle::check_config_radius()
{ {
// ensure radius is at least as large as vehicle's turn radius // ensure radius is at least as large as vehicle's turn radius
if (config.radius < rover.g2.turn_radius) { if (config.radius < g2.turn_radius) {
config.radius = rover.g2.turn_radius; config.radius = g2.turn_radius;
gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)rover.g2.turn_radius); gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)g2.turn_radius);
} }
} }

2
Rover/mode_guided.cpp
View File

@ -443,5 +443,5 @@ bool ModeGuided::limit_breached() const
// scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
bool ModeGuided::use_scurves_for_navigation() const bool ModeGuided::use_scurves_for_navigation() const
{ {
return ((rover.g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0); return ((g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0);
} }

6
Rover/mode_loiter.cpp
View File

@ -23,16 +23,16 @@ void ModeLoiter::update()
// get distance (in meters) to destination // get distance (in meters) to destination
_distance_to_destination = rover.current_loc.get_distance(_destination); _distance_to_destination = rover.current_loc.get_distance(_destination);
const float loiter_radius = rover.g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius; const float loiter_radius = g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
// if within loiter radius slew desired speed towards zero and use existing desired heading // if within loiter radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= loiter_radius) { if (_distance_to_destination <= loiter_radius) {
// sailboats should not stop unless motoring // sailboats should not stop unless motoring
const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f; const float desired_speed_within_radius = g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt); _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
// if we have a sail but not trying to use it then point into the wind // if we have a sail but not trying to use it then point into the wind
if (!rover.g2.sailboat.tack_enabled() && rover.g2.sailboat.sail_enabled()) { if (!g2.sailboat.tack_enabled() && g2.sailboat.sail_enabled()) {
_desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f; _desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f;
} }
} else { } else {