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

Copter: support circle with terrain altitude

Browse Source
This commit is contained in:
Randy Mackay 2020-04-08 15:21:13 +09:00
parent 658bb646ca
commit 71cbedb178
3 changed files with 8 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/mode_auto.cpp
View File

@ -243,14 +243,8 @@ void ModeAuto::land_start(const Vector3f& destination)
// we assume the caller has performed all required GPS_ok checks
void ModeAuto::circle_movetoedge_start(const Location &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();
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
}
copter.circle_nav->set_center(circle_center_neu);
// set circle center
copter.circle_nav->set_center(circle_center);
// set circle radius
if (!is_zero(radius_m)) {
@ -280,6 +274,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &circle_center_neu = copter.circle_nav->get_center();
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
// initialise yaw
@ -304,7 +299,7 @@ void ModeAuto::circle_start()
_mode = Auto_Circle;
// initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center());
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
@ -831,7 +826,7 @@ void ModeAuto::rtl_run()
void ModeAuto::circle_run()
{
// call circle controller
copter.circle_nav->update();
copter.failsafe_terrain_set_status(copter.circle_nav->update());
// call z-axis position controller
pos_control->update_z_controller();

2
ArduCopter/mode_circle.cpp
View File

@ -104,7 +104,7 @@ void ModeCircle::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run circle controller
copter.circle_nav->update();
copter.failsafe_terrain_set_status(copter.circle_nav->update());
// call attitude controller
if (pilot_yaw_override) {

3
ArduCopter/sensors.cpp
View File

@ -90,10 +90,11 @@ void Copter::read_rangefinder(void)
rf_state.last_healthy_ms = now;
}
// send downward facing lidar altitude and health to waypoint navigation library
// send downward facing lidar altitude and health to waypoint and circle navigation libraries
if (rf_orient == ROTATION_PITCH_270) {
if (rangefinder_state.alt_healthy || timed_out) {
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
circle_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
}
}
}