本文整理汇总了C++中constrain_float函数的典型用法代码示例。如果您正苦于以下问题:C++ constrain_float函数的具体用法?C++ constrain_float怎么用?C++ constrain_float使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了constrain_float函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: switch
//.........这里部分代码省略.........
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
rover.ahrs.set_home(new_home_loc);
rover.home_is_set = HOME_SET_NOT_LOCKED;
rover.Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(new_home_loc);
result = MAV_RESULT_ACCEPTED;
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat * 1.0e-7f),
(double)(new_home_loc.lng * 1.0e-7f),
(uint32_t)(new_home_loc.alt * 0.01f));
}
break;
}
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL:
result = rover.compass.handle_mag_cal_command(packet);
break;
case MAV_CMD_NAV_SET_YAW_SPEED:
{
// param1 : yaw angle to adjust direction by in centidegress
// param2 : Speed - normalized to 0 .. 1
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
rover.guided_mode = Guided_Angle;
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
rover.guided_yaw_speed.turn_angle = packet.param1;
rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
rover.nav_set_yaw_speed();
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED;
if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
result = MAV_RESULT_ACCEPTED;
}
break;
default:
break;
}
mavlink_msg_command_ack_send_buf(
msg,
chan,
packet.command,
result);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
break;
}
开发者ID:njoubert,项目名称:ardupilot,代码行数:67,代码来源:GCS_Mavlink.cpp
示例2: constrain_float
void AP_TECS::_update_throttle(void)
{
// Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem;
// Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast;
_STEdotErrLast = STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed)
{
_throttle_dem = 1.0f;
}
else
{
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
float maxAmp = 0.5f*(_THRmaxf - _THRminf);
float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
{
_integ6_state = integ_max;
}
else
{
_integ6_state = constrain_float(_integ6_state, integ_min, integ_max);
}
// Sum the components.
// Only use feed-forward component if airspeed is not being used
if (_ahrs.airspeed_sensor_enabled()) {
_throttle_dem = _throttle_dem + _integ6_state;
} else {
_throttle_dem = ff_throttle;
}
}
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
}
开发者ID:AloshkaD,项目名称:ardupilot,代码行数:86,代码来源:AP_TECS.cpp
示例3: radians
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
// Sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis
// angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the
// input value and the feedforward rate is zeroed.
_att_target_euler_rad.x = euler_roll_angle_rad;
_att_target_euler_rate_rads.x = 0;
}
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis
// angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
_att_target_euler_rad.y = euler_pitch_angle_rad;
_att_target_euler_rate_rads.y = 0;
}
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
if (_rate_bf_ff_enabled) {
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
} else {
euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads);
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
}
开发者ID:9DSmart,项目名称:ardupilot,代码行数:76,代码来源:AC_AttitudeControl.cpp
示例4: constrain_float
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
float rate_ef_desired;
float rate_change_limit;
Vector3f angle_ef_error; // earth frame angle errors
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// if accel limiting and feed forward enabled
if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) {
rate_change_limit = _accel_roll_max * _dt;
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max);
// apply acceleration limit to feed forward roll rate
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
// update earth-frame roll angle target using desired roll rate
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
// if accel limiting and feed forward enabled
if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) {
rate_change_limit = _accel_pitch_max * _dt;
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_pitch_max);
// apply acceleration limit to feed forward pitch rate
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
// update earth-frame pitch angle target using desired pitch rate
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.y = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
if (_accel_yaw_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_yaw_max * _dt;
// update yaw rate target with acceleration limit
_rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit);
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = yaw_rate_ef;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
}
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
} else {
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
开发者ID:Mikebert4,项目名称:ardupilot,代码行数:93,代码来源:AC_AttitudeControl.cpp
示例5: fabsf
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// calculate dt
const uint32_t now = AP_HAL::millis();
float dt = (now - _steer_turn_last_ms) / 1000.0f;
if (_steer_turn_last_ms == 0 || dt > 0.1f) {
dt = 0.0f;
_steer_rate_pid.reset_filter();
} else {
_steer_rate_pid.set_dt(dt);
}
_steer_turn_last_ms = now;
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// we expect caller will not try to control heading using rate control without a valid speed estimate
// on failure to get speed we do not attempt to steer
return 0.0f;
}
// only use positive speed. Use reverse flag instead of negative speeds.
speed = fabsf(speed);
// enforce minimum speed to stop oscillations when first starting to move
bool low_speed = false;
if (speed < AR_ATTCONTROL_STEER_SPEED_MIN) {
low_speed = true;
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
// scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles
float scaler = 1.0f;
if (!skid_steering) {
scaler = 1.0f / fabsf(speed);
}
// Calculate the steering rate error (rad/sec) and apply gain scaler
// We do this in earth frame to allow for rover leaning over in hard corners
float yaw_rate_earth = _ahrs.get_yaw_rate_earth();
// check if reversing
if (reversed) {
yaw_rate_earth *= -1.0f;
}
const float rate_error = (desired_rate - yaw_rate_earth) * scaler;
// record desired rate for logging purposes only
_steer_rate_pid.set_desired_rate(desired_rate);
// pass error to PID controller
_steer_rate_pid.set_input_filter_all(rate_error);
// get p
const float p = _steer_rate_pid.get_p();
// get i unless non-skid-steering rover at low speed or steering output has hit a limit
float i = _steer_rate_pid.get_integrator();
if ((!low_speed || skid_steering) && ((is_negative(rate_error) && !motor_limit_left) || (is_positive(rate_error) && !motor_limit_right))) {
i = _steer_rate_pid.get_i();
}
// get d
const float d = _steer_rate_pid.get_d();
// constrain and return final output
return constrain_float(p + i + d, -1.0f, 1.0f);
}
开发者ID:ElcoCuijpers,项目名称:ardupilot,代码行数:69,代码来源:AR_AttitudeControl.cpp
示例6: switch
void Rover::update_current_mode(void)
{
switch (control_mode){
case AUTO:
case RTL:
set_reverse(false);
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
case GUIDED:
set_reverse(false);
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
}
break;
case STEERING: {
/*
in steering mode we control lateral acceleration
directly. We first calculate the maximum lateral
acceleration at full steering lock for this speed. That is
V^2/R where R is the radius of turn. We get the radius of
turn from half the STEER2SRV_P.
*/
float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
calc_nav_steer();
// and throttle gives speed in proportion to cruise speed, up
// to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise;
set_reverse(target_speed < 0);
if (in_reverse) {
target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
} else {
target_speed = constrain_float(target_speed, 0, g.speed_cruise);
}
calc_throttle(target_speed);
break;
}
case LEARNING:
case MANUAL:
/*
in both MANUAL and LEARNING we pass through the
controls. Setting servo_out here actually doesn't matter, as
we set the exact value in set_servos(), but it helps for
logging
*/
channel_throttle->set_servo_out(channel_throttle->get_control_in());
channel_steer->set_servo_out(channel_steer->pwm_to_angle());
// mark us as in_reverse when using a negative throttle to
// stop AHRS getting off
set_reverse(channel_throttle->get_servo_out() < 0);
break;
case HOLD:
// hold position - stop motors and center steering
channel_throttle->set_servo_out(0);
channel_steer->set_servo_out(0);
set_reverse(false);
break;
case INITIALISING:
break;
}
}
开发者ID:Annashuang,项目名称:ardupilot,代码行数:84,代码来源:APMrover2.cpp
示例7: sqrt_controller
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad)
{
Vector3f rate_target_ang_vel;
// Compute the roll angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
}
return rate_target_ang_vel;
}
开发者ID:BrendanSmith,项目名称:ardupilot,代码行数:26,代码来源:AC_AttitudeControl.cpp
示例8: get_node
void AP_UAVCAN::do_cyclic(void)
{
if (_initialized) {
auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
if (error < 0) {
hal.scheduler->delay_microseconds(1000);
} else {
if (rc_out_sem_take()) {
if (_rco_armed) {
bool repeat_send;
// if we have any Servos in bitmask
if (_servo_bm > 0) {
uint8_t starting_servo = 0;
do {
repeat_send = false;
uavcan::equipment::actuator::ArrayCommand msg;
uint8_t i;
// UAVCAN can hold maximum of 15 commands in one frame
for (i = 0; starting_servo < UAVCAN_RCO_NUMBER && i < 15; starting_servo++) {
uavcan::equipment::actuator::Command cmd;
/*
* Servo output uses a range of 1000-2000 PWM for scaling.
* This converts output PWM from [1000:2000] range to [-1:1] range that
* is passed to servo as unitless type via UAVCAN.
* This approach allows for MIN/TRIM/MAX values to be used fully on
* autopilot side and for servo it should have the setup to provide maximum
* physically possible throws at [-1:1] limits.
*/
if (_rco_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
cmd.actuator_id = starting_servo + 1;
// TODO: other types
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
// TODO: failsafe, safety
cmd.command_value = constrain_float(((float) _rco_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
msg.commands.push_back(cmd);
i++;
}
}
if (i > 0) {
act_out_array[_uavcan_i]->broadcast(msg);
if (i == 15) {
repeat_send = true;
}
}
} while (repeat_send);
}
// if we have any ESC's in bitmask
if (_esc_bm > 0) {
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
uavcan::equipment::esc::RawCommand esc_msg;
uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0;
// find out how many esc we have enabled and if they are active at all
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
if ((((uint32_t) 1) << i) & _esc_bm) {
max_esc_num = i + 1;
if (_rco_conf[i].active) {
active_esc_num++;
}
}
}
// if at least one is active (update) we need to send to all
if (active_esc_num > 0) {
k = 0;
for (uint8_t i = 0; i < max_esc_num && k < 20; i++) {
uavcan::equipment::actuator::Command cmd;
if ((((uint32_t) 1) << i) & _esc_bm) {
// TODO: ESC negative scaling for reverse thrust and reverse rotation
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_rco_conf[i].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
esc_msg.cmd.push_back(static_cast<int>(scaled));
} else {
esc_msg.cmd.push_back(static_cast<unsigned>(0));
}
k++;
}
//.........这里部分代码省略.........
开发者ID:BrendanSmith,项目名称:ardupilot,代码行数:101,代码来源:AP_UAVCAN.cpp
示例9: while
//.........这里部分代码省略.........
velE,
velD,
posN,
posE,
posD,
gyrX,
gyrY,
gyrZ);
// define messages for EKF2 data packet
int8_t accWeight = (int8_t)(100*accelWeighting);
int8_t acc1 = (int8_t)(100*accelZBias1);
int8_t acc2 = (int8_t)(100*accelZBias2);
int16_t windN = (int16_t)(100*windVel.x);
int16_t windE = (int16_t)(100*windVel.y);
int16_t magN = (int16_t)(magNED.x);
int16_t magE = (int16_t)(magNED.y);
int16_t magD = (int16_t)(magNED.z);
int16_t magX = (int16_t)(magXYZ.x);
int16_t magY = (int16_t)(magXYZ.y);
int16_t magZ = (int16_t)(magXYZ.z);
// print EKF2 data packet
fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n",
AP_HAL::millis() * 0.001f,
AP_HAL::millis(),
accWeight,
acc1,
acc2,
windN,
windE,
magN,
magE,
magD,
magX,
magY,
magZ);
// define messages for EKF3 data packet
int16_t innovVN = (int16_t)(100*velInnov.x);
int16_t innovVE = (int16_t)(100*velInnov.y);
int16_t innovVD = (int16_t)(100*velInnov.z);
int16_t innovPN = (int16_t)(100*posInnov.x);
int16_t innovPE = (int16_t)(100*posInnov.y);
int16_t innovPD = (int16_t)(100*posInnov.z);
int16_t innovMX = (int16_t)(magInnov.x);
int16_t innovMY = (int16_t)(magInnov.y);
int16_t innovMZ = (int16_t)(magInnov.z);
int16_t innovVT = (int16_t)(100*tasInnov);
// print EKF3 data packet
fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n",
AP_HAL::millis() * 0.001f,
AP_HAL::millis(),
innovVN,
innovVE,
innovVD,
innovPN,
innovPE,
innovPD,
innovMX,
innovMY,
innovMZ,
innovVT);
// define messages for EKF4 data packet
int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX));
int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX));
int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX));
int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX));
int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX));
int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX));
int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX));
int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX));
int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX));
// print EKF4 data packet
fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n",
AP_HAL::millis() * 0.001f,
(unsigned)AP_HAL::millis(),
(int)sqrtvarV,
(int)sqrtvarP,
(int)sqrtvarH,
(int)sqrtvarMX,
(int)sqrtvarMY,
(int)sqrtvarMZ,
(int)sqrtvarVT,
(int)offsetNorth,
(int)offsetEast,
(int)faultStatus);
}
}
flush_dataflash();
if (check_solution) {
report_checks();
}
exit(0);
}
开发者ID:uzzndw,项目名称:ardupilot,代码行数:101,代码来源:Replay.cpp
示例10: update_simple_mode
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::ModeLoiter::run()
{
LoiterModeState loiter_state;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in());
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
} else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
wp_nav->clear_pilot_desired_acceleration();
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav->loiter_soften_for_landing();
}
// Loiter State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
loiter_state = Loiter_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
loiter_state = Loiter_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
loiter_state = Loiter_Landed;
} else {
loiter_state = Loiter_Flying;
}
// Loiter State Machine
switch (loiter_state) {
case Loiter_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
#else
wp_nav->init_loiter_target();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
pos_control->update_z_controller();
break;
case Loiter_Takeoff:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control->update_z_controller();
break;
case Loiter_Landed:
//.........这里部分代码省略.........
开发者ID:FantasyJXF,项目名称:ardupilot,代码行数:101,代码来源:mode_loiter.cpp
示例11: MAX
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum FlightStage flight_stage,
bool is_doing_auto_land,
float distance_beyond_land_wp,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor)
{
// Calculate time in seconds since last update
uint32_t now = AP_HAL::micros();
_DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
_flags.is_doing_auto_land = is_doing_auto_land;
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
// Update the speed estimate using a 2nd order complementary filter
_update_speed(load_factor);
if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else {
_THRmaxf = aparm.throttle_max * 0.01f;
}
_THRminf = aparm.throttle_min * 0.01f;
// work out the maximum and minimum pitch
// if TECS_PITCH_{MAX,MIN} isn't set then use
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
// larger than LIM_PITCH_{MAX,MIN}
if (_pitch_max <= 0) {
_PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
} else {
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
}
// apply temporary pitch limit and clear
_PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
_pitch_max_limit = 90;
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else {
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
}
if (flight_stage == FLIGHT_LAND_FINAL) {
// in flare use min pitch from LAND_PITCH_CD
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max != 0) {
_PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
}
// and allow zero throttle
_THRminf = 0;
} else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) {
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point
float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
if (time_to_flare < 0) {
// we should be flaring already
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
} else if (time_to_flare < timeConstant()*2) {
// smoothly move the min pitch to the flare min pitch over
// twice the time constant
float p = time_to_flare/(2*timeConstant());
float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
#if 0
::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
_PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
}
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
// Calculate the speed demand
_update_speed_demand();
// Calculate the height demand
_update_height_demand();
//.........这里部分代码省略.........
开发者ID:Amnesiac2,项目名称:ardupilot,代码行数:101,代码来源:AP_TECS.cpp
示例12: constrain_float
void AP_TECS::_update_pitch(void)
{
// Calculate Speed/Height Control Weighting
// This is used to determine how the pitch control prioritises speed and height control
// A weighting of 1 provides equal priority (this is the normal mode of operation)
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed
// rises above the demanded value, the pitch angle will be increased by the TECS controller.
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH ||
_flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE ||
_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
log_tuning.speed_weight = SKE_weighting;
float SPE_weighting = 2.0f - SKE_weighting;
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem);
}
else if (_pitch_dem < _PITCHminf)
{
integ7_input = MAX(integ7_input, _PITCHminf - _pitch_dem);
}
_integ7_state = _integ7_state + integ7_input * _DT;
#if 0
if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) {
::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n",
_hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem),
SEB_dem, SEBdot_dem, SEB_error, SEBdot_error);
}
#endif
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
// During flare a different damping gain is used
float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_dem * timeConstant();
float pitch_damp = _ptchDamp;
if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
pitch_damp = _landDamp;
} else if (!is_zero(_land_pitch_damp) && is_on_land_approach(false)) {
pitch_damp = _land_pitch_damp;
}
temp += SEBdot_error * pitch_damp;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
// Rate limit the pitch demand to comply with specified vertical
// acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _integ5_state;
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr)
{
_pitch_dem = _last_pitch_dem + ptchRateIncr;
}
else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr)
{
_pitch_dem = _last_pitch_dem - ptchRateIncr;
}
// re-constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf);
//.........这里部分代码省略.........
开发者ID:Amnesiac2,项目名称:ardupilot,代码行数:101,代码来源:AP_TECS.cpp
示例13: switch
void AP_MotorsMulticopter::output_logic()
{
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_multicopter_flags.spool_mode = SPIN_WH
|
请发表评论