/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) { // capture time since last iteration uint32_t now = hal.scheduler->millis(); float dt = (now - _last_update_xy_ms) / 1000.0f; // sanity check dt - expect to be called faster than ~5hz if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) { dt = 0.0f; } // check if xy leash needs to be recalculated calc_leash_length_xy(); // apply desired velocity request to position target desired_vel_to_pos(dt); // run position controller's position error to desired velocity step pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler); // run velocity to acceleration step rate_to_accel_xy(dt, ekfNavVelGainScaler); // run acceleration to lean angle step accel_to_lean_angles(dt, ekfNavVelGainScaler, false); // update altitude target set_alt_target_from_climb_rate(_vel_desired.z, dt, false); // run z-axis position controller update_z_controller(); // record update time _last_update_xy_ms = now; }
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors void AC_PosControl::update_vel_controller_xyz() { update_vel_controller_xy(); // update altitude target set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false); // run z-axis position controller update_z_controller(); }
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors void AC_PosControl::update_vel_controller_xyz() { // capture time since last iteration uint32_t now = hal.scheduler->millis(); float dt_xy = (now - _last_update_vel_xyz_ms) / 1000.0f; // check if xy leash needs to be recalculated calc_leash_length_xy(); // we will run the horizontal component every 4th iteration (i.e. 50hz on Pixhawk, 20hz on APM) if (dt_xy >= POSCONTROL_VEL_UPDATE_TIME) { // record update time _last_update_vel_xyz_ms = now; // sanity check dt if (dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS) { dt_xy = 0.0f; } // apply desired velocity request to position target desired_vel_to_pos(dt_xy); // run position controller's position error to desired velocity step pos_to_rate_xy(true, dt_xy); // run velocity to acceleration step rate_to_accel_xy(dt_xy); // run acceleration to lean angle step accel_to_lean_angles(); } // update altitude target set_alt_target_from_climb_rate(_vel_desired.z, _dt); // run z-axis position controller update_z_controller(); }
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) { // capture time since last iteration uint32_t now = AP_HAL::millis(); float dt = (now - _last_update_xy_ms)*0.001f; // call xy controller if (dt >= get_dt_xy()) { // sanity check dt if (dt >= 0.2f) { dt = 0.0f; } // check if xy leash needs to be recalculated calc_leash_length_xy(); // apply desired velocity request to position target desired_vel_to_pos(dt); // run position controller's position error to desired velocity step pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler); // run velocity to acceleration step rate_to_accel_xy(dt, ekfNavVelGainScaler); // run acceleration to lean angle step accel_to_lean_angles(dt, ekfNavVelGainScaler, false); // update xy update time _last_update_xy_ms = now; } // update altitude target set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false); // run z-axis position controller update_z_controller(); }