Copter: completely disable vel controller

This commit is contained in:
Randy Mackay 2014-10-23 20:54:44 +09:00
parent 5cbcbf9b37
commit 43c5a70424
2 changed files with 11 additions and 2 deletions

View File

@ -64,6 +64,7 @@ void guided_pos_control_start()
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
#if NAV_GUIDED == ENABLED
// initialise guided mode's velocity controller
void guided_vel_control_start()
{
@ -77,6 +78,7 @@ void guided_vel_control_start()
// initialise velocity controller
pos_control.init_vel_controller_xyz();
}
#endif
// guided_set_destination - sets guided mode's target destination
static void guided_set_destination(const Vector3f& destination)
@ -89,6 +91,7 @@ static void guided_set_destination(const Vector3f& destination)
wp_nav.set_wp_destination(destination);
}
#if NAV_GUIDED == ENABLED
// guided_set_velocity - sets guided mode's target velocity
static void guided_set_velocity(const Vector3f& velocity)
{
@ -100,6 +103,7 @@ static void guided_set_velocity(const Vector3f& velocity)
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
}
#endif
// guided_run - runs the guided controller
// should be called at 100hz or more
@ -128,9 +132,12 @@ static void guided_run()
guided_pos_control_run();
break;
#if NAV_GUIDED == ENABLED
case Guided_Velocity:
// run velocity controller
guided_vel_control_run();
break;
#endif
}
}

View File

@ -195,8 +195,10 @@ enum AutoMode {
// Guided modes
enum GuidedMode {
Guided_TakeOff,
Guided_WP,
Guided_Velocity
Guided_WP
#if NAV_GUIDED == ENABLED
,Guided_Velocity
#endif
};
// RTL states