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

Copter: do-circle accept terrain altitude

Browse Source
This commit is contained in:
Randy Mackay 2016-03-09 20:21:08 +09:00
parent 70630e9774
commit 81d244c9bd
3 changed files with 70 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -725,7 +725,7 @@ private:
void auto_land_run();
void auto_rtl_start();
void auto_rtl_run();
void auto_circle_movetoedge_start();
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
void auto_circle_start();
void auto_circle_run();
void auto_nav_guided_start();

60
ArduCopter/commands_logic.cpp
View File

@ -373,43 +373,33 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
Location_Class circle_center(cmd.content.location);
// default lat/lon to current position if not provided
// To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
if (circle_center.lat == 0 && circle_center.lng == 0) {
circle_center.lat = current_loc.lat;
circle_center.lng = current_loc.lng;
}
// default target altitude to current altitude if not provided
if (circle_center.alt == 0) {
int32_t curr_alt;
if (current_loc.get_alt_cm(circle_center.get_alt_frame(),curr_alt)) {
// circle altitude uses frame from command
circle_center.set_alt(curr_alt,circle_center.get_alt_frame());
} else {
// default to current altitude above origin
circle_center.set_alt(current_loc.alt, current_loc.get_alt_frame());
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// calculate radius
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
bool move_to_edge_required = false;
// set target altitude if not provided
if (cmd.content.location.alt == 0) {
circle_center.z = curr_pos.z;
} else {
move_to_edge_required = true;
}
// set lat/lon position if not provided
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
} else {
move_to_edge_required = true;
}
// set circle controller's center
circle_nav.set_center(circle_center);
// set circle radius
if (circle_radius_m != 0) {
circle_nav.set_radius((float)circle_radius_m * 100.0f);
}
// check if we need to move to edge of circle
if (move_to_edge_required) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start();
} else {
// start circling
auto_circle_start();
}
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start(circle_center, circle_radius_m);
}
// do_loiter_time - initiate loitering at a point for a given time period

60
ArduCopter/control_auto.cpp
View File

@ -453,37 +453,65 @@ void Copter::auto_rtl_run()
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start()
void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
}
circle_nav.set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav.set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
Vector3f circle_edge_neu;
circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// convert altitude to same as command
circle_edge.set_alt(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination(circle_edge)) {
// 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
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_circle_start();
}
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
// assumes that circle_nav object has already been initialised with circle center and radius
void Copter::auto_circle_start()
{
auto_mode = Auto_Circle;
// initialise circle controller
// center was set in do_circle so initialise with current center
circle_nav.init(circle_nav.get_center());
}