Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 04:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: auto takeoff accepts abs and terrain alts

Browse Source
This commit is contained in:
Randy Mackay 2015-10-05 17:23:58 +09:00
parent 9449776e3c
commit 06ee6a7bd4
3 changed files with 38 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -714,7 +714,7 @@ private:
void althold_run();
bool auto_init(bool ignore_checks);
void auto_run();
void auto_takeoff_start(float final_alt_above_home);
void auto_takeoff_start(const Location& dest_loc);
void auto_takeoff_run();
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location_Class& dest_loc);

5
ArduCopter/commands_logic.cpp
View File

@ -284,10 +284,7 @@ void Copter::do_RTL(void)
void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
// Set wp navigation target to safe altitude above current position
float takeoff_alt = cmd.content.location.alt;
takeoff_alt = MAX(takeoff_alt,current_loc.alt);
takeoff_alt = MAX(takeoff_alt,100.0f);
auto_takeoff_start(takeoff_alt);
auto_takeoff_start(cmd.content.location);
}
// do_nav_wp - initiate move to next waypoint

42
ArduCopter/control_auto.cpp
View File

@ -91,14 +91,42 @@ void Copter::auto_run()
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(float final_alt_above_home)
void Copter::auto_takeoff_start(const Location& dest_loc)
{
auto_mode = Auto_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);
// convert location to class
Location_Class dest(dest_loc);
// set horizontal target
dest.lat = current_loc.lat;
dest.lng = current_loc.lng;
// get altitude target
int32_t alt_target;
if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) {
// this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude
alt_target = current_loc.alt + dest.alt;
}
// sanity check target
if (alt_target < current_loc.alt) {
dest.set_alt(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
}
// Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
if (alt_target < 100) {
dest.set_alt(100, Location_Class::ALT_FRAME_ABOVE_HOME);
}
// set waypoint controller target
if (!wp_nav.set_wp_destination(dest)) {
// this should never fail because the target altitude is specified as an alt-above-home
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
return;
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -140,7 +168,9 @@ void Copter::auto_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();