aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: spline mission commands handle terrain altitudes

Browse Source
This commit is contained in:
Randy Mackay 2016-03-11 16:45:00 +09:00
parent 81d244c9bd
commit e8b14e59fc
3 changed files with 43 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -1028,7 +1028,7 @@ private:
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void log_init(void);

41
ArduCopter/commands_logic.cpp
View File

@ -417,8 +417,23 @@ void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
// do_spline_wp - initiate move to next waypoint
void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f& curr_pos = inertial_nav.get_position();
Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
Location_Class 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(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt(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;
@ -429,7 +444,6 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
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;
Vector3f next_destination; // end of next segment
// 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?
@ -443,19 +457,34 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
}
// if there is no delay at the end of this segment get next nav command
Location_Class 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(next_alt, next_loc.get_alt_frame());
} else {
// default to first waypoints altitude
next_loc.set_alt(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;
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
}
}
// set spline navigation target
auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination);
auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
}
#if NAV_GUIDED == ENABLED

10
ArduCopter/control_auto.cpp
View File

@ -269,14 +269,18 @@ void Copter::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 Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
void Copter::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Vector3f& next_destination)
const Location_Class& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination (likely because of missing terrain data)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
}
// 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