From 267c1c3934e816979e963d4557a6fec1e05a0168 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2016 17:16:26 +0900 Subject: [PATCH] Copter: guided mode handles terrain alt --- ArduCopter/Copter.h | 3 ++- ArduCopter/commands_logic.cpp | 9 +++---- ArduCopter/control_guided.cpp | 46 +++++++++++++++++++++++++++++------ ArduCopter/takeoff.cpp | 8 +++--- 4 files changed, 50 insertions(+), 16 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 47078b17a4..f08249d79f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -771,12 +771,13 @@ private: bool flip_init(bool ignore_checks); void flip_run(); bool guided_init(bool ignore_checks); - void guided_takeoff_start(float final_alt_above_home); + bool guided_takeoff_start(float final_alt_above_home); void guided_pos_control_start(); void guided_vel_control_start(); void guided_posvel_control_start(); void guided_angle_control_start(); void guided_set_destination(const Vector3f& destination); + bool guided_set_destination(const Location_Class& dest_loc); void guided_set_velocity(const Vector3f& velocity); void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); void guided_set_angle(const Quaternion &q, float climb_rate_cms); diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 4221caf98e..fae96d29e5 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -809,8 +809,6 @@ bool Copter::verify_yaw() // do_guided - start guided mode bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { - Vector3f pos_or_vel; // target location or velocity - // only process guided waypoint if we are in guided mode if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { return false; @@ -820,11 +818,12 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: + { // set wp_nav's destination - pos_or_vel = pv_location_to_vector(cmd.content.location); - guided_set_destination(pos_or_vel); - return true; + Location_Class dest(cmd.content.location); + return guided_set_destination(dest); break; + } case MAV_CMD_CONDITION_YAW: do_yaw(cmd); diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 9c8e326565..899bab1270 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -51,20 +51,26 @@ bool Copter::guided_init(bool ignore_checks) // guided_takeoff_start - initialises waypoint controller to implement take-off -void Copter::guided_takeoff_start(float final_alt_above_home) +bool Copter::guided_takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; - + // initialise wpnav destination - Vector3f target_pos = inertial_nav.get_position(); - target_pos.z = pv_alt_above_origin(final_alt_above_home); - wp_nav.set_wp_destination(target_pos); + Location_Class target_loc = current_loc; + target_loc.set_alt(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); + + if (!wp_nav.set_wp_destination(target_loc)) { + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + return false; + } // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); + + return true; } // initialise guided mode's position controller @@ -169,6 +175,26 @@ void Copter::guided_set_destination(const Vector3f& destination) Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); } +// sets guided mode's target from a Location object +// returns false if destination could not be set (probably caused by missing terrain data) +bool Copter::guided_set_destination(const Location_Class& dest_loc) +{ + // ensure we are in position control mode + if (guided_mode != Guided_WP) { + guided_pos_control_start(); + } + + if (!wp_nav.set_wp_destination(dest_loc)) { + // failure to set destination (likely because of missing terrain data) + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + return false; + } + + // log target + Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + return true; +} + // guided_set_velocity - sets guided mode's target velocity void Copter::guided_set_velocity(const Vector3f& velocity) { @@ -289,7 +315,9 @@ void Copter::guided_takeoff_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + if (wp_nav.update_wpnav()) { + // To-Do: handle failure to update probably caused by missing terrain data + } // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -330,7 +358,11 @@ void Copter::guided_pos_control_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + if (!wp_nav.update_wpnav()) { + // failures to update probably caused by missing terrain data + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET); + // To-Do: handle failure by triggering RTL after 5 seconds with no update? + } // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index ca158eb93f..ca7f3e2b41 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -38,9 +38,11 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) switch(control_mode) { case GUIDED: - set_auto_armed(true); - guided_takeoff_start(takeoff_alt_cm); - return true; + if (guided_takeoff_start(takeoff_alt_cm)) { + set_auto_armed(true); + return true; + } + return false; case LOITER: case POSHOLD: case ALT_HOLD: