/// update_wpnav - run the wp controller - should be called at 10hz void AC_WPNav::update_wpnav() { // calculate dt uint32_t now = hal.scheduler->millis(); float dt = (now - _wpnav_last_update) / 1000.0f; // catch if we've just been started if( dt >= 1.0f ) { dt = 0.0; reset_I(); _wpnav_step = 0; } else if (_wpnav_reset > 0) { _wpnav_step = 0; _wpnav_reset = 0; _accel_reset = 1; } // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle if (dt > 0.095f && _wpnav_step > 3) { _wpnav_step = 0; } // run loiter steps switch (_wpnav_step) { case 0: // capture time since last iteration _wpnav_dt = dt; _wpnav_last_update = now; // advance the target if necessary if (dt > 0.0f) { advance_target_along_track(dt); } _wpnav_step++; break; case 1: // run loiter's position to velocity step get_loiter_position_to_velocity(_wpnav_dt, _wp_speed_cms); _wpnav_step++; break; case 2: // run loiter's velocity to acceleration step get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _wpnav_dt); _wpnav_step++; break; case 3: // run loiter's acceleration to lean angle step get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y); _wpnav_step++; break; } }
/// update_wpnav - run the wp controller - should be called at 10hz void AC_WPNav::update_wpnav() { uint32_t now = hal.scheduler->millis(); float dt = (now - _wpnav_last_update) / 1000.0f; _wpnav_last_update = now; // catch if we've just been started if( dt >= 1.0 ) { dt = 0.0; reset_I(); }else{ // advance the target if necessary advance_target_along_track(dt); } // run loiter position controller get_loiter_position_to_velocity(dt, _wp_speed_cms); }