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

Rover: integrate position controller

This commit is contained in:
Randy Mackay 2021-11-19 12:38:12 +09:00
parent 9d629f5ecd
commit 95c69811cb
9 changed files with 74 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Rover/Parameters.cpp
View File

@ -282,10 +282,6 @@ const AP_Param::Info Rover::var_info[] = {
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
GOBJECT(serial_manager, "SERIAL", AP_SerialManager),
// @Group: NAVL1_
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
GOBJECT(rangefinder, "RNGFND", RangeFinder),
@ -660,6 +656,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(ais, "AIS_", 50, ParametersG2, AP_AIS),
#endif
// @Group: PSC
// @Path: ../libraries/APM_Control/AR_PosControl.cpp
AP_SUBGROUPINFO(pos_control, "PSC", 51, ParametersG2, AR_PosControl),
AP_GROUPEND
};
@ -709,7 +709,8 @@ ParametersG2::ParametersG2(void)
avoid(),
follow(),
windvane(),
wp_nav(attitude_control, rover.L1_controller),
pos_control(attitude_control),
wp_nav(attitude_control, pos_control),
sailboat()
{
AP_Param::setup_object_defaults(this, var_info);
@ -744,7 +745,6 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" },
{ Parameters::k_param_pivot_turn_angle_old, 0, AP_PARAM_INT16, "WP_PIVOT_ANGLE" },
{ Parameters::k_param_waypoint_radius_old, 0, AP_PARAM_FLOAT, "WP_RADIUS" },
{ Parameters::k_param_waypoint_overshoot_old, 0, AP_PARAM_FLOAT, "WP_OVERSHOOT" },
{ Parameters::k_param_g2, 20, AP_PARAM_INT16, "WP_PIVOT_RATE" },
{ Parameters::k_param_g2, 32, AP_PARAM_FLOAT, "SAIL_ANGLE_MIN" },
@ -792,9 +792,6 @@ void Rover::load_parameters(void)
SRV_Channels::upgrade_parameters();
hal.console->printf("load_all took %uus\n", unsigned(micros() - before));
// set a more reasonable default NAVL1_PERIOD for rovers
L1_controller.set_default_period(NAVL1_PERIOD);
// convert CH7_OPTION to RC7_OPTION for Rover-3.4 to 3.5 upgrade
const AP_Param::ConversionInfo ch7_option_info = { Parameters::k_param_ch7_option, 0, AP_PARAM_INT8, "RC7_OPTION" };
AP_Int8 ch7_opt_old;

5
Rover/Parameters.h
View File

@ -206,7 +206,7 @@ public:
k_param_ins,
k_param_compass,
k_param_rcmap,
k_param_L1_controller,
k_param_L1_controller, // unused
k_param_steerController_old, // unused
k_param_barometer,
k_param_notify,
@ -411,6 +411,9 @@ public:
// Automatic Identification System - for tracking sea-going vehicles
AP_AIS ais;
#endif
// position controller
AR_PosControl pos_control;
};
extern const AP_Param::Info var_info[];

6
Rover/Rover.cpp
View File

@ -371,6 +371,11 @@ void Rover::update_logging1(void)
if (should_log(MASK_LOG_NTUN)) {
Log_Write_Nav_Tuning();
if (g2.pos_control.is_active()) {
g2.pos_control.write_log();
logger.Write_PID(LOG_PIDN_MSG, g2.pos_control.get_vel_pid().get_pid_info_x());
logger.Write_PID(LOG_PIDE_MSG, g2.pos_control.get_vel_pid().get_pid_info_y());
}
}
#if HAL_PROXIMITY_ENABLED
@ -433,6 +438,7 @@ void Rover::one_second_loop(void)
// send latest param values to wp_nav
g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
}
void Rover::update_current_mode(void)

4
Rover/Rover.h
View File

@ -34,7 +34,6 @@
#include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Declination/AP_Declination.h> // Compass declination library
#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_L1_Control/AP_L1_Control.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
@ -52,6 +51,7 @@
#include <AP_WheelEncoder/AP_WheelEncoder.h>
#include <AP_WheelEncoder/AP_WheelRateControl.h>
#include <APM_Control/AR_AttitudeControl.h>
#include <APM_Control/AR_PosControl.h>
#include <AR_WPNav/AR_WPNav.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
#include <AP_Logger/AP_Logger.h>
@ -156,8 +156,6 @@ private:
// Arming/Disarming management class
AP_Arming_Rover arming;
AP_L1_Control L1_controller{ahrs, nullptr};
#if AP_OPTICALFLOW_ENABLED
OpticalFlow optflow;
#endif

4
Rover/mode.cpp
View File

@ -246,9 +246,9 @@ float Mode::get_desired_lat_accel() const
}
// set desired location
bool Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
bool Mode::set_desired_location(const Location &destination, Location next_destination )
{
if (!g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
if (!g2.wp_nav.set_desired_location(destination, next_destination)) {
return false;
}

8
Rover/mode.h
View File

@ -112,8 +112,8 @@ public:
virtual bool get_desired_location(Location& destination) const WARN_IF_UNUSED { return false; }
// set desired location (used in Guided, Auto)
// next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
// set next_destination (if known). If not provided vehicle stops at destination
virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
virtual bool reached_destination() const { return true; }
@ -262,7 +262,7 @@ public:
// get or set desired location
bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
bool reached_destination() const override;
// set desired speed in m/s
@ -413,7 +413,7 @@ public:
// get or set desired location
bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
// set desired heading and speed
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);

48
Rover/mode_auto.cpp
View File

@ -61,17 +61,18 @@ void ModeAuto::update()
switch (_submode) {
case Auto_WP:
{
if (!g2.wp_nav.reached_destination()) {
// check if we've reached the destination
if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
// update navigation controller
navigate_to_waypoint();
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
start_stop();
}
} else {
stop_vehicle();
start_stop();
}
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
@ -187,10 +188,10 @@ bool ModeAuto::get_desired_location(Location& destination) const
}
// set desired location to drive to
bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
bool ModeAuto::set_desired_location(const Location &destination, Location next_destination)
{
// call parent
if (!Mode::set_desired_location(destination, next_leg_bearing_cd)) {
if (!Mode::set_desired_location(destination, next_destination)) {
return false;
}
@ -602,29 +603,36 @@ void ModeAuto::do_RTL(void)
bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
{
// get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
// in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN;
if (!always_stop_at_destination && loiter_duration == 0) {
next_leg_bearing_cd = mission.get_next_ground_course_cd(AR_WPNAV_HEADING_UNKNOWN);
}
// retrieve and sanitize target location
Location cmdloc = cmd.content.location;
cmdloc.sanitize(rover.current_loc);
if (!set_desired_location(cmdloc, next_leg_bearing_cd)) {
return false;
// delayed stored in p1 in seconds
loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1;
loiter_start_time = 0;
if (loiter_duration > 0) {
always_stop_at_destination = true;
}
// do not add next wp if there are no more navigation commands
AP_Mission::Mission_Command next_cmd;
if (always_stop_at_destination || !mission.get_next_nav_cmd(cmd.index+1, next_cmd)) {
// single destination
if (!set_desired_location(cmdloc)) {
return false;
}
} else {
// retrieve and sanitize next destination location
Location next_cmdloc = next_cmd.content.location;
next_cmdloc.sanitize(cmdloc);
if (!set_desired_location(cmdloc, next_cmdloc)) {
return false;
}
}
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_start_time = 0;
// this is the delay, stored in seconds, checked such that commanded delays < 0 delay 0 seconds
loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1;
return true;
}

5
Rover/mode_guided.cpp
View File

@ -232,10 +232,9 @@ bool ModeGuided::get_desired_location(Location& destination) const
}
// set desired location
bool ModeGuided::set_desired_location(const struct Location& destination,
float next_leg_bearing_cd)
bool ModeGuided::set_desired_location(const Location &destination, Location next_destination)
{
if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
if (g2.wp_nav.set_desired_location(destination, next_destination)) {
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;

29
Rover/mode_smart_rtl.cpp
View File

@ -49,20 +49,33 @@ void ModeSmartRTL::update()
case SmartRTL_PathFollow:
// load point if required
if (_load_point) {
Vector3f next_point;
if (!g2.smart_rtl.pop_point(next_point)) {
Vector3f dest_NED;
if (!g2.smart_rtl.pop_point(dest_NED)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
break;
} else {
// peek at the next point. this can fail if the IO task currently has the path semaphore
Vector3f next_dest_NED;
if (g2.smart_rtl.peek_point(next_dest_NED)) {
if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// no next point so add only immediate point
if (!g2.wp_nav.set_desired_location_NED(dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
}
_load_point = false;
// set target destination to new point
if (!g2.wp_nav.set_desired_location_NED(next_point)) {
// this failure should never happen but we add it just in case
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
}
}
// update navigation controller
navigate_to_waypoint();