From e8b14e59fcee9a71d7e53afb24ad713de435e0d5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2016 16:45:00 +0900 Subject: [PATCH] Copter: spline mission commands handle terrain altitudes --- ArduCopter/Copter.h | 2 +- ArduCopter/commands_logic.cpp | 41 ++++++++++++++++++++++++++++++----- ArduCopter/control_auto.cpp | 10 ++++++--- 3 files changed, 43 insertions(+), 10 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 81fd666853..47078b17a4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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); diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 2cf16771e3..4221caf98e 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -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 diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 47d10a00bf..177aa68652 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -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