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

Sub: integrate s-curves, remove spline support

This commit is contained in:
Randy Mackay 2020-06-10 09:15:51 +09:00
parent ff8b69fbad
commit afa59b9a60
5 changed files with 9 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/Sub.h
View File

@ -463,7 +463,6 @@ private:
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location& dest_loc);
void auto_wp_run();
void auto_spline_run();
void auto_circle_movetoedge_start(const Location &circle_center, float radius_m);
void auto_circle_start();
void auto_circle_run();
@ -577,7 +576,6 @@ private:
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_circle(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
@ -595,13 +593,11 @@ private:
bool verify_surface(const AP_Mission::Mission_Command& cmd);
bool verify_RTL(void);
bool verify_circle(const AP_Mission::Mission_Command& cmd);
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
void auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
void log_init(void);
void accel_cal_update(void);
void read_airspeed();

109
ArduSub/commands_logic.cpp
View File

@ -55,10 +55,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
@ -167,9 +163,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
@ -252,11 +245,6 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// Set wp navigation target
auto_wp_start(target_loc);
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0) {
wp_nav.set_fast_waypoint(true);
}
}
// do_surface - initiate surface procedure
@ -383,79 +371,6 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
loiter_time_max = cmd.p1; // units are (seconds)
}
// do_spline_wp - initiate move to next waypoint
void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
Location target_loc(cmd.content.location);
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
target_loc.lat = current_loc.lat;
target_loc.lng = current_loc.lng;
}
// use current altitude if not provided
if (target_loc.alt == 0) {
// set to current altitude but in command's alt frame
int32_t curr_alt;
if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
}
}
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds
loiter_time_max = cmd.p1;
// determine segment start and end type
bool stopped_at_start = true;
AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
AP_Mission::Mission_Command temp_cmd;
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
stopped_at_start = false;
}
}
}
// if there is no delay at the end of this segment get next nav command
Location next_loc;
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
next_loc = temp_cmd.content.location;
// default lat, lon to first waypoint's lat, lon
if (next_loc.lat == 0 && next_loc.lng == 0) {
next_loc.lat = target_loc.lat;
next_loc.lng = target_loc.lng;
}
// default alt to first waypoint's alt but in next waypoint's alt frame
if (next_loc.alt == 0) {
int32_t next_alt;
if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) {
next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame());
} else {
// default to first waypoints altitude
next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame());
}
}
// if the next nav command is a waypoint set end type to spline or straight
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
} else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
}
}
// set spline navigation target
auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
}
#if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer
void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
@ -464,7 +379,7 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// initialise guided limits
guided_limit_init_time_and_pos();
// set spline navigation target
// set navigation target
auto_nav_guided_start();
}
}
@ -618,28 +533,6 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
// verify_spline_wp - check if we have reached the next way point using spline
bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if (!wp_nav.reached_wp_destination()) {
return false;
}
// start timer if necessary
if (loiter_time == 0) {
loiter_time = AP_HAL::millis();
}
// check if timer has run out
if (((AP_HAL::millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true;
}
return false;
}
#if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)

94
ArduSub/control_auto.cpp
View File

@ -23,7 +23,7 @@ bool Sub::auto_init()
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
// initialise waypoint controller
wp_nav.wp_and_spline_init();
// clear guided limits
@ -53,10 +53,6 @@ void Sub::auto_run()
auto_circle_run();
break;
case Auto_Spline:
auto_spline_run();
break;
case Auto_NavGuided:
#if NAV_GUIDED == ENABLED
auto_nav_guided_run();
@ -94,7 +90,7 @@ void Sub::auto_wp_start(const Location& dest_loc)
auto_mode = Auto_WP;
// send target to waypoint controller
if (!wp_nav.set_wp_destination(dest_loc)) {
if (!wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
@ -172,83 +168,6 @@ void Sub::auto_wp_run()
}
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Sub::auto_spline_start(const Location& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Location& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Sub::auto_spline_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_spline();
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
}
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
@ -279,7 +198,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination(circle_edge)) {
if (!wp_nav.set_wp_destination_loc(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
@ -362,15 +281,12 @@ bool Sub::auto_loiter_start()
}
auto_mode = Auto_Loiter;
Vector3f origin = inertial_nav.get_position();
// calculate stopping point
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
wp_nav.get_wp_stopping_point(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
wp_nav.set_wp_destination(stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);

6
ArduSub/control_guided.cpp
View File

@ -51,7 +51,7 @@ void Sub::guided_pos_control_start()
// set to position control mode
guided_mode = Guided_WP;
// initialise waypoint and spline controller
// initialise waypoint controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
@ -181,7 +181,7 @@ bool Sub::guided_set_destination(const Location& dest_loc)
}
#endif
if (!wp_nav.set_wp_destination(dest_loc)) {
if (!wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
@ -386,7 +386,7 @@ void Sub::guided_vel_control_run()
}
}
// guided_posvel_control_run - runs the guided spline controller
// guided_posvel_control_run - runs the guided posvel controller
// called from guided_run
void Sub::guided_posvel_control_run()
{

1
ArduSub/defines.h
View File

@ -60,7 +60,6 @@ enum AutoMode {
Auto_WP,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline,
Auto_NavGuided,
Auto_Loiter,
Auto_TerrainRecover