mirror of https://github.com/ArduPilot/ardupilot
Copter: support for SCurves and position controller changes
wp_start provides next_dest_loc send next_destination to wp_nav instead of setting fast_waypoint fixup zigzag for S-curve changes fixup guided auto spline fixes smart rtl rename of next_point to dest_NED loc_from_cmd accepts default location auto mode stops before starting land command auto do_next_wp accepts default location rename do_next_wp to set_next_wp also rename get_spline_from_cmd argument also improve failure to set next waypoint due to missing terrain data also fixup comment in set_next_wp also auto stops when moving from straight to spline segments also auto mode spline fix also auto mode calls AC_WPNav::set_spline_destination_next Copter: AutoYaw provides rate from WPNav
This commit is contained in:
Leonard Hall
Randy Mackay
parent
204c839ae0
commit
328c0655e3
|
|
@ -215,8 +215,21 @@ float Mode::AutoYaw::yaw()
|
|||
// messages (positive is clockwise, negative is counter clockwise)
|
||||
float Mode::AutoYaw::rate_cds() const
|
||||
{
|
||||
if (_mode == AUTO_YAW_RATE) {
|
||||
switch (_mode) {
|
||||
|
||||
case AUTO_YAW_HOLD:
|
||||
case AUTO_YAW_ROI:
|
||||
case AUTO_YAW_FIXED:
|
||||
case AUTO_YAW_LOOK_AHEAD:
|
||||
case AUTO_YAW_RESETTOARMEDYAW:
|
||||
case AUTO_YAW_CIRCLE:
|
||||
return 0.0f;
|
||||
|
||||
case AUTO_YAW_RATE:
|
||||
return _rate_cds;
|
||||
|
||||
case AUTO_YAW_LOOK_AT_NEXT_WP:
|
||||
return copter.wp_nav->get_yaw_rate();
|
||||
}
|
||||
|
||||
// return zero turn rate (this should never happen)
|
||||
|
|
|
|||
|
|
@ -91,7 +91,6 @@ enum AutoMode {
|
|||
Auto_RTL,
|
||||
Auto_CircleMoveToEdge,
|
||||
Auto_Circle,
|
||||
Auto_Spline,
|
||||
Auto_NavGuided,
|
||||
Auto_Loiter,
|
||||
Auto_LoiterToAlt,
|
||||
|
|
|
|||
|
|
@ -369,8 +369,6 @@ public:
|
|||
void land_start(const Vector3f& destination);
|
||||
void circle_movetoedge_start(const Location &circle_center, float radius_m);
|
||||
void circle_start();
|
||||
void spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
|
||||
void spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
|
||||
void nav_guided_start();
|
||||
|
||||
bool is_landing() const override;
|
||||
|
|
@ -419,7 +417,6 @@ private:
|
|||
|
||||
void takeoff_run();
|
||||
void wp_run();
|
||||
void spline_run();
|
||||
void land_run();
|
||||
void rtl_run();
|
||||
void circle_run();
|
||||
|
|
@ -427,7 +424,7 @@ private:
|
|||
void loiter_run();
|
||||
void loiter_to_alt_run();
|
||||
|
||||
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd) const;
|
||||
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
|
||||
|
||||
void payload_place_start(const Vector3f& destination);
|
||||
void payload_place_run();
|
||||
|
|
@ -442,12 +439,14 @@ private:
|
|||
|
||||
void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
|
||||
void do_land(const AP_Mission::Mission_Command& cmd);
|
||||
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_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
|
||||
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline);
|
||||
#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);
|
||||
|
|
|
|||
|
|
@ -93,10 +93,6 @@ void ModeAuto::run()
|
|||
circle_run();
|
||||
break;
|
||||
|
||||
case Auto_Spline:
|
||||
spline_run();
|
||||
break;
|
||||
|
||||
case Auto_NavGuided:
|
||||
#if NAV_GUIDED == ENABLED
|
||||
nav_guided_run();
|
||||
|
|
@ -190,7 +186,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
|
|||
}
|
||||
|
||||
// set waypoint controller target
|
||||
if (!wp_nav->set_wp_destination(dest)) {
|
||||
if (!wp_nav->set_wp_destination_loc(dest)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
copter.failsafe_terrain_on_event();
|
||||
return;
|
||||
|
|
@ -212,7 +208,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
|
|||
_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
|
||||
copter.failsafe_terrain_on_event();
|
||||
return;
|
||||
|
|
@ -293,7 +289,7 @@ void ModeAuto::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
|
||||
copter.failsafe_terrain_on_event();
|
||||
}
|
||||
|
|
@ -331,28 +327,6 @@ void ModeAuto::circle_start()
|
|||
}
|
||||
}
|
||||
|
||||
// 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 ModeAuto::spline_start(const Location& destination, bool stopped_at_start,
|
||||
AC_WPNav::spline_segment_end_type seg_end_type,
|
||||
const Location& next_destination)
|
||||
{
|
||||
_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
|
||||
copter.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) {
|
||||
auto_yaw.set_mode_to_default(false);
|
||||
}
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
|
||||
void ModeAuto::nav_guided_start()
|
||||
|
|
@ -782,47 +756,7 @@ void ModeAuto::wp_run()
|
|||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
|
||||
} else {
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
|
||||
}
|
||||
}
|
||||
|
||||
// auto_spline_run - runs the auto spline controller
|
||||
// called by auto_run at 100hz or more
|
||||
void ModeAuto::spline_run()
|
||||
{
|
||||
// if not armed set throttle to zero and exit immediately
|
||||
if (is_disarmed_or_landed()) {
|
||||
make_safe_spool_down();
|
||||
wp_nav->wp_and_spline_init();
|
||||
return;
|
||||
}
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!copter.failsafe.radio && use_pilot_yaw()) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
if (!is_zero(target_yaw_rate)) {
|
||||
auto_yaw.set_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();
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// 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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
|
||||
} else {
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), auto_yaw.rate_cds());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1096,25 +1030,25 @@ void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
|||
takeoff_start(cmd.content.location);
|
||||
}
|
||||
|
||||
Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
|
||||
Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const
|
||||
{
|
||||
Location ret(cmd.content.location);
|
||||
|
||||
// use current lat, lon if zero
|
||||
if (ret.lat == 0 && ret.lng == 0) {
|
||||
ret.lat = copter.current_loc.lat;
|
||||
ret.lng = copter.current_loc.lng;
|
||||
ret.lat = default_loc.lat;
|
||||
ret.lng = default_loc.lng;
|
||||
}
|
||||
// use current altitude if not provided
|
||||
if (ret.alt == 0) {
|
||||
// set to current altitude but in command's alt frame
|
||||
int32_t curr_alt;
|
||||
if (copter.current_loc.get_alt_cm(ret.get_alt_frame(),curr_alt)) {
|
||||
ret.set_alt_cm(curr_alt, ret.get_alt_frame());
|
||||
// set to default_loc's altitude but in command's alt frame
|
||||
// note that this may use the terrain database
|
||||
int32_t default_alt;
|
||||
if (default_loc.get_alt_cm(ret.get_alt_frame(), default_alt)) {
|
||||
ret.set_alt_cm(default_alt, ret.get_alt_frame());
|
||||
} else {
|
||||
// default to current altitude as alt-above-home
|
||||
ret.set_alt_cm(copter.current_loc.alt,
|
||||
copter.current_loc.get_alt_frame());
|
||||
// default to default_loc's altitude and frame
|
||||
ret.set_alt_cm(default_loc.alt, default_loc.get_alt_frame());
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
|
|
@ -1123,46 +1057,90 @@ Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
|
|||
// do_nav_wp - initiate move to next waypoint
|
||||
void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Location target_loc = loc_from_cmd(cmd);
|
||||
// calculate default location used when lat, lon or alt is zero
|
||||
Location default_loc = copter.current_loc;
|
||||
if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
|
||||
if (!wp_nav->get_wp_destination_loc(default_loc)) {
|
||||
// this should never happen
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
}
|
||||
|
||||
// get waypoint's location from command and send to wp_nav
|
||||
const Location dest_loc = loc_from_cmd(cmd, default_loc);
|
||||
if (!wp_nav->set_wp_destination_loc(dest_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
copter.failsafe_terrain_on_event();
|
||||
return;
|
||||
}
|
||||
|
||||
_mode = Auto_WP;
|
||||
|
||||
// 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;
|
||||
|
||||
// Set wp navigation target
|
||||
wp_start(target_loc);
|
||||
|
||||
// if no delay as well as not final waypoint set the waypoint as "fast"
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
bool fast_waypoint = false;
|
||||
if (loiter_time_max == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
|
||||
|
||||
// whether vehicle should stop at the target position depends upon the next command
|
||||
switch (temp_cmd.id) {
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
case MAV_CMD_NAV_LOITER_TURNS:
|
||||
case MAV_CMD_NAV_LOITER_TIME:
|
||||
case MAV_CMD_NAV_LAND:
|
||||
case MAV_CMD_NAV_SPLINE_WAYPOINT:
|
||||
// if next command's lat, lon is specified then do not slowdown at this waypoint
|
||||
if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) {
|
||||
fast_waypoint = true;
|
||||
}
|
||||
break;
|
||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||
// do not stop for RTL
|
||||
fast_waypoint = true;
|
||||
break;
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
default:
|
||||
// always stop for takeoff commands
|
||||
// for unsupported commands it is safer to stop
|
||||
break;
|
||||
}
|
||||
copter.wp_nav->set_fast_waypoint(fast_waypoint);
|
||||
// set next destination if necessary
|
||||
if (!set_next_wp(cmd, dest_loc)) {
|
||||
// failure to set next destination can only be because of missing terrain data
|
||||
copter.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) {
|
||||
auto_yaw.set_mode_to_default(false);
|
||||
}
|
||||
}
|
||||
|
||||
// checks the next mission command and adds it as a destination if necessary
|
||||
// supports both straight line and spline waypoints
|
||||
// cmd should be the current command
|
||||
// default_loc should be the destination from the current_cmd but corrected for cases where user set lat, lon or alt to zero
|
||||
// returns true on success, false on failure which should only happen due to a failure to retrieve terrain data
|
||||
bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc)
|
||||
{
|
||||
// do not add next wp if current command has a delay meaning the vehicle will stop at the destination
|
||||
if (current_cmd.p1 > 0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// do not add next wp if there are no more navigation commands
|
||||
AP_Mission::Mission_Command next_cmd;
|
||||
if (!mission.get_next_nav_cmd(current_cmd.index+1, next_cmd)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// whether vehicle should stop at the target position depends upon the next command
|
||||
switch (next_cmd.id) {
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
case MAV_CMD_NAV_LOITER_TURNS:
|
||||
case MAV_CMD_NAV_LOITER_TIME: {
|
||||
const Location dest_loc = loc_from_cmd(current_cmd, default_loc);
|
||||
const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc);
|
||||
return wp_nav->set_wp_destination_next_loc(next_dest_loc);
|
||||
}
|
||||
case MAV_CMD_NAV_SPLINE_WAYPOINT: {
|
||||
// get spline's location and next location from command and send to wp_nav
|
||||
Location next_dest_loc, next_next_dest_loc;
|
||||
bool next_next_dest_loc_is_spline;
|
||||
get_spline_from_cmd(next_cmd, default_loc, next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline);
|
||||
return wp_nav->set_spline_destination_next_loc(next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline);
|
||||
}
|
||||
case MAV_CMD_NAV_LAND:
|
||||
// stop because we may change between rel,abs and terrain alt types
|
||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
// always stop for RTL and takeoff commands
|
||||
default:
|
||||
// for unsupported commands it is safer to stop
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// do_land - initiate landing procedure
|
||||
|
|
@ -1225,7 +1203,7 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
|||
// do_circle - initiate moving in a circle
|
||||
void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
const Location circle_center = loc_from_cmd(cmd);
|
||||
const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
|
||||
|
||||
// calculate radius
|
||||
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
|
||||
|
|
@ -1276,67 +1254,65 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
|
|||
pos_control->set_max_accel_z(wp_nav->get_accel_z());
|
||||
pos_control->set_max_speed_z(wp_nav->get_default_speed_down(),
|
||||
wp_nav->get_default_speed_up());
|
||||
|
||||
if (pos_control->is_active_z()) {
|
||||
pos_control->freeze_ff_z();
|
||||
}
|
||||
}
|
||||
|
||||
// do_spline_wp - initiate move to next waypoint
|
||||
// do_nav_wp - initiate move to next waypoint
|
||||
void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
const Location target_loc = loc_from_cmd(cmd);
|
||||
// calculate default location used when lat, lon or alt is zero
|
||||
Location default_loc = copter.current_loc;
|
||||
if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
|
||||
if (!wp_nav->get_wp_destination_loc(default_loc)) {
|
||||
// this should never happen
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
}
|
||||
|
||||
// get spline's location and next location from command and send to wp_nav
|
||||
Location dest_loc, next_dest_loc;
|
||||
bool next_dest_loc_is_spline;
|
||||
get_spline_from_cmd(cmd, default_loc, dest_loc, next_dest_loc, next_dest_loc_is_spline);
|
||||
if (!wp_nav->set_spline_destination_loc(dest_loc, next_dest_loc, next_dest_loc_is_spline)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
copter.failsafe_terrain_on_event();
|
||||
return;
|
||||
}
|
||||
|
||||
_mode = Auto_WP;
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
// set next destination if necessary
|
||||
if (!set_next_wp(cmd, dest_loc)) {
|
||||
// failure to set next destination can only be because of missing terrain data
|
||||
copter.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) {
|
||||
auto_yaw.set_mode_to_default(false);
|
||||
}
|
||||
}
|
||||
|
||||
// get_spline_from_cmd - Calculates the spline type for a given spline waypoint mission command
|
||||
void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline)
|
||||
{
|
||||
dest_loc = loc_from_cmd(cmd, default_loc);
|
||||
|
||||
// if there is no delay at the end of this segment get next nav command
|
||||
Location next_loc;
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
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;
|
||||
}
|
||||
next_dest_loc = loc_from_cmd(temp_cmd, dest_loc);
|
||||
next_dest_loc_is_spline = temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT;
|
||||
} else {
|
||||
next_dest_loc = dest_loc;
|
||||
next_dest_loc_is_spline = false;
|
||||
}
|
||||
|
||||
// set spline navigation target
|
||||
spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
|
|
|
|||
|
|
@ -116,7 +116,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
|
|||
}
|
||||
target_loc.set_alt_cm(takeoff_alt_cm, frame);
|
||||
|
||||
if (!wp_nav->set_wp_destination(target_loc)) {
|
||||
if (!wp_nav->set_wp_destination_loc(target_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
|
||||
|
|
@ -296,7 +296,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
|
|||
pos_control_start();
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -123,14 +123,13 @@ void ModeRTL::climb_start()
|
|||
}
|
||||
|
||||
// set the destination
|
||||
if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
|
||||
if (!wp_nav->set_wp_destination_loc(rtl_path.climb_target) || !wp_nav->set_wp_destination_next_loc(rtl_path.return_target)) {
|
||||
// this should not happen because rtl_build_path will have checked terrain data was available
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target");
|
||||
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
|
||||
copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE);
|
||||
return;
|
||||
}
|
||||
wp_nav->set_fast_waypoint(true);
|
||||
|
||||
// hold current yaw during initial climb
|
||||
auto_yaw.set_mode(AUTO_YAW_HOLD);
|
||||
|
|
@ -142,7 +141,7 @@ void ModeRTL::return_start()
|
|||
_state = RTL_ReturnHome;
|
||||
_state_complete = false;
|
||||
|
||||
if (!wp_nav->set_wp_destination(rtl_path.return_target)) {
|
||||
if (!wp_nav->set_wp_destination_loc(rtl_path.return_target)) {
|
||||
// failure must be caused by missing terrain data, restart RTL
|
||||
restart_without_terrain();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -92,24 +92,26 @@ void ModeSmartRTL::path_follow_run()
|
|||
|
||||
// if we are close to current target point, switch the next point to be our target.
|
||||
if (wp_nav->reached_wp_destination()) {
|
||||
Vector3f next_point;
|
||||
Vector3f dest_NED;
|
||||
// this pop_point can fail if the IO task currently has the
|
||||
// path semaphore.
|
||||
if (g2.smart_rtl.pop_point(next_point)) {
|
||||
if (g2.smart_rtl.pop_point(dest_NED)) {
|
||||
path_follow_last_pop_fail_ms = 0;
|
||||
if (g2.smart_rtl.get_num_points() == 0) {
|
||||
// this is the very last point, add 2m to the target alt and move to pre-land state
|
||||
next_point.z -= 2.0f;
|
||||
dest_NED.z -= 2.0f;
|
||||
smart_rtl_state = SmartRTL_PreLandPosition;
|
||||
wp_nav->set_wp_destination_NED(next_point);
|
||||
wp_nav->set_wp_destination_NED(dest_NED);
|
||||
} else {
|
||||
// peek at the next point
|
||||
Vector3f next_next_point;
|
||||
if (g2.smart_rtl.peek_point(next_next_point)) {
|
||||
wp_nav->set_wp_destination_NED(next_point, next_next_point);
|
||||
Vector3f next_dest_NED;
|
||||
if (g2.smart_rtl.peek_point(next_dest_NED)) {
|
||||
wp_nav->set_wp_destination_NED(dest_NED);
|
||||
wp_nav->set_wp_destination_next_NED(next_dest_NED);
|
||||
} else {
|
||||
// this should never happen but send next point anyway
|
||||
wp_nav->set_wp_destination_NED(next_point);
|
||||
wp_nav->set_wp_destination_NED(dest_NED);
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
}
|
||||
} else if (g2.smart_rtl.get_num_points() == 0) {
|
||||
|
|
@ -181,7 +183,7 @@ bool ModeSmartRTL::get_wp(Location& destination)
|
|||
case SmartRTL_PathFollow:
|
||||
case SmartRTL_PreLandPosition:
|
||||
case SmartRTL_Descend:
|
||||
return wp_nav->get_wp_destination(destination);
|
||||
return wp_nav->get_wp_destination_loc(destination);
|
||||
case SmartRTL_Land:
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ void Copter::tuning()
|
|||
break;
|
||||
|
||||
case TUNING_THROTTLE_RATE_KP:
|
||||
pos_control->get_vel_z_p().kP(tuning_value);
|
||||
pos_control->get_vel_z_pid().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_ACCEL_Z_KP:
|
||||
|
|
|
|||
Loading…
Reference in New Issue