zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: integrate s-curves, remove spline support

zr-v5.1
Randy Mackay 5 years ago
parent
ff8b69fbad
commit
afa59b9a60
5 changed files with 9 additions and 205 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduSub/Sub.h
  2. 109
      ArduSub/commands_logic.cpp
  3. 94
      ArduSub/control_auto.cpp
  4. 6
      ArduSub/control_guided.cpp
  5. 1
      ArduSub/defines.h

4
ArduSub/Sub.h
Unescape View File

@ -463,7 +463,6 @@ private: @@ -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: @@ -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: @@ -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
Unescape View File

@ -55,10 +55,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -23,7 +23,7 @@ bool Sub::auto_init() @@ -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() @@ -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) @@ -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() @@ -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 @@ -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() @@ -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
Unescape View File

@ -51,7 +51,7 @@ void Sub::guided_pos_control_start() @@ -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) @@ -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() @@ -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
Unescape View File

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

Write Preview
Loading…
Cancel
Save