Skip to content

Commit 9ba6018

Browse files
committed
ArduCopter: fix MAV_CMD_DO_ORBIT double speed bug
orbit_run() was calling update_ms() twice per loop iteration: once before the turn-count check and once after. Each call advances the circle angle and ramps angular velocity, resulting in exactly 2x the requested speed. Fix by removing the redundant first call and keeping only the single update_ms(0.0f) call before the attitude/position controller update.
1 parent aebddf1 commit 9ba6018

1 file changed

Lines changed: 1 addition & 4 deletions

File tree

ArduCopter/mode_guided.cpp

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1358,8 +1358,7 @@ void ModeGuided::orbit_apply_yaw_behaviour()
13581358
void ModeGuided::orbit_run()
13591359
{
13601360
// call circle controller
1361-
copter.failsafe_terrain_set_status(copter.circle_nav->update_ms());
1362-
1361+
copter.failsafe_terrain_set_status(copter.circle_nav->update_ms(0.0f));
13631362
// check if we've completed the requested number of turns
13641363
if (is_positive(_orbit_turns)) {
13651364
const float turns_completed = fabsf(copter.circle_nav->get_angle_total_rad() / float(M_2PI));
@@ -1370,9 +1369,7 @@ void ModeGuided::orbit_run()
13701369
return;
13711370
}
13721371
}
1373-
13741372
// call attitude, position and yaw controllers
1375-
copter.failsafe_terrain_set_status(copter.circle_nav->update_ms(0.0f));
13761373
pos_control->D_update_controller();
13771374
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
13781375
}

0 commit comments

Comments
 (0)