Skip to content
Snippets Groups Projects
Commit 43c5a704 authored by Randy Mackay's avatar Randy Mackay
Browse files

Copter: completely disable vel controller

parent 5cbcbf9b
No related branches found
No related tags found
No related merge requests found
...@@ -64,6 +64,7 @@ void guided_pos_control_start() ...@@ -64,6 +64,7 @@ void guided_pos_control_start()
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} }
#if NAV_GUIDED == ENABLED
// initialise guided mode's velocity controller // initialise guided mode's velocity controller
void guided_vel_control_start() void guided_vel_control_start()
{ {
...@@ -77,6 +78,7 @@ void guided_vel_control_start() ...@@ -77,6 +78,7 @@ void guided_vel_control_start()
// initialise velocity controller // initialise velocity controller
pos_control.init_vel_controller_xyz(); pos_control.init_vel_controller_xyz();
} }
#endif
// guided_set_destination - sets guided mode's target destination // guided_set_destination - sets guided mode's target destination
static void guided_set_destination(const Vector3f& destination) static void guided_set_destination(const Vector3f& destination)
...@@ -89,6 +91,7 @@ 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); wp_nav.set_wp_destination(destination);
} }
#if NAV_GUIDED == ENABLED
// guided_set_velocity - sets guided mode's target velocity // guided_set_velocity - sets guided mode's target velocity
static void guided_set_velocity(const Vector3f& velocity) static void guided_set_velocity(const Vector3f& velocity)
{ {
...@@ -100,6 +103,7 @@ 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 // set position controller velocity target
pos_control.set_desired_velocity(velocity); pos_control.set_desired_velocity(velocity);
} }
#endif
// guided_run - runs the guided controller // guided_run - runs the guided controller
// should be called at 100hz or more // should be called at 100hz or more
...@@ -128,9 +132,12 @@ static void guided_run() ...@@ -128,9 +132,12 @@ static void guided_run()
guided_pos_control_run(); guided_pos_control_run();
break; break;
#if NAV_GUIDED == ENABLED
case Guided_Velocity: case Guided_Velocity:
// run velocity controller // run velocity controller
guided_vel_control_run(); guided_vel_control_run();
break;
#endif
} }
} }
......
...@@ -195,8 +195,10 @@ enum AutoMode { ...@@ -195,8 +195,10 @@ enum AutoMode {
// Guided modes // Guided modes
enum GuidedMode { enum GuidedMode {
Guided_TakeOff, Guided_TakeOff,
Guided_WP, Guided_WP
Guided_Velocity #if NAV_GUIDED == ENABLED
,Guided_Velocity
#endif
}; };
// RTL states // RTL states
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment