mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: guided mode handles terrain alt
This commit is contained in:
parent
e8b14e59fc
commit
267c1c3934
@ -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);
|
||||
|
@ -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);
|
||||
|
@ -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();
|
||||
|
@ -38,9 +38,11 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||
|
||||
switch(control_mode) {
|
||||
case GUIDED:
|
||||
if (guided_takeoff_start(takeoff_alt_cm)) {
|
||||
set_auto_armed(true);
|
||||
guided_takeoff_start(takeoff_alt_cm);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
case LOITER:
|
||||
case POSHOLD:
|
||||
case ALT_HOLD:
|
||||
|
Loading…
Reference in New Issue
Block a user