/* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector<3> rates; rates(0) = _ctrl_state.roll_rate; rates(1) = _ctrl_state.pitch_rate; rates(2) = _ctrl_state.yaw_rate; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */ if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } } } }
/* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ if (!_armed.armed) { _rates_int.zero(); } /* current body angular rates */ math::Vector<3> rates; rates(0) = _v_att.rollspeed; rates(1) = _v_att.pitchspeed; rates(2) = _v_att.yawspeed; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; /* update integral only if not saturated on low limit */ if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } } } }
/* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector<3> rates; rates(0) = _ctrl_state.roll_rate; rates(1) = _ctrl_state.pitch_rate; rates(2) = _ctrl_state.yaw_rate; /* throttle pid attenuation factor */ float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint))); /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; _att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */ if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT && /* if the axis is the yaw axis, do not update the integral if the limit is hit */ !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) { _rates_int(i) = rate_i; } } } } }
void MulticopterPositionControl::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); /* * do subscriptions */ _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); parameters_update(true); /* initialize values of critical structs until first regular update */ _arming.armed = false; /* get an initial update for all sensor and status data */ poll_subscriptions(); bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool reset_yaw_sp = true; bool was_armed = false; hrt_abstime t_prev = 0; _hover_time = 0.0; // miao: _mode_mission = 1; math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; R.identity(); /* wakeup source */ struct pollfd fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } poll_subscriptions(); parameters_update(false); hrt_abstime t = hrt_absolute_time(); float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; reset_yaw_sp = true; _reset_mission = true;//miao: } //Update previous arming state was_armed = _control_mode.flag_armed; update_ref(); if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; _vel(0) = _local_pos.vx; _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; _vel_ff.zero(); _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); _mode_auto = false; } else { /* AUTO */ control_auto(dt); } if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } else { /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } /* use constant descend rate when landing, ignore altitude setpoint */ //if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { // miao: for auto landing test with manual mode if (_mode_mission==3) { _vel_sp(2) = _params.land_speed; } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); /* publish velocity setpoint */ if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; float i = _params.thr_min; if (reset_int_z_manual) { i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; } else if (i > _params.thr_max) { i = _params.thr_max; } } thrust_int(2) = -i; } } else { reset_int_z = true; } if (_control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; } } else { reset_int_xy = true; } /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; } if (!_control_mode.flag_control_climb_rate_enabled) { thrust_sp(2) = 0.0f; } /* limit thrust vector and check for saturation */ bool saturation_xy = false; bool saturation_z = false; /* limit min lift */ float thr_min = _params.thr_min; if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { /* don't allow downside thrust direction in manual attitude mode */ thr_min = 0.0f; } float tilt_max = _params.tilt_max_air; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; if (thr_min < 0.0f) { thr_min = 0.0f; } } /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; saturation_z = true; } if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } } } else { /* thrust compensation for altitude only control mode */ float att_comp; if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { att_comp = 1.0f / PX4_R(_att.R, 2, 2); } else if (PX4_R(_att.R, 2, 2) > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { att_comp = 1.0f; saturation_z = true; } thrust_sp(2) *= att_comp; } /* limit max thrust */ float thrust_abs = thrust_sp.length(); if (thrust_abs > _params.thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > _params.thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; thrust_sp(2) = -_params.thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } else { /* Z component is negative, going down, simply limit thrust vector */ float k = _params.thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } thrust_abs = _params.thr_max; } /* update integrals */ if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; } if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; } } /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ math::Vector<3> body_x; math::Vector<3> body_y; math::Vector<3> body_z; if (thrust_abs > SIGMA) { body_z = -thrust_sp / thrust_abs; } else { /* no thrust, set Z axis to safe value */ body_z.zero(); body_z(2) = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); if (fabsf(body_z(2)) > SIGMA) { /* desired body_x axis, orthogonal to body_z */ body_x = y_C % body_z; /* keep nose to front while inverted upside down */ if (body_z(2) < 0.0f) { body_x = -body_x; } body_x.normalize(); } else { /* desired thrust is in XY plane, set X downside to construct correct matrix, * but yaw component will not be used actually */ body_x.zero(); body_x(2) = 1.0f; } /* desired body_y axis */ body_y = body_z % body_x; /* fill rotation matrix */ for (int i = 0; i < 3; i++) { R(i, 0) = body_x(i); R(i, 1) = body_y(i); R(i, 2) = body_z(i); } /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } else if (!_control_mode.flag_control_manual_enabled) { /* autonomous altitude control without position control (failsafe landing), * force level attitude, don't change yaw */ R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; /* save thrust setpoint for logging */ _local_pos_sp.acc_x = thrust_sp(0); _local_pos_sp.acc_x = thrust_sp(1); _local_pos_sp.acc_x = thrust_sp(2); _att_sp.timestamp = hrt_absolute_time(); } else { reset_int_z = true; } } /* fill local position, velocity and thrust setpoint */ _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); _local_pos_sp.yaw = _att_sp.yaw_body; _local_pos_sp.vx = _vel_sp(0); _local_pos_sp.vy = _vel_sp(1); _local_pos_sp.vz = _vel_sp(2); /* publish local position setpoint */ if (_local_pos_sp_pub > 0) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; _mode_auto = false; reset_int_z = true; reset_int_xy = true; } // generate attitude setpoint from manual controls if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { // reset yaw setpoint to current position if needed if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _att.yaw; } // do not move yaw while arming else if (_manual.z > 0.1f) { const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); if (yaw_offs < - YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); } else if (yaw_offs > YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); } } //Control roll and pitch directly if we no aiding velocity controller is active if(!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = _manual.z; } //Construct attitude setpoint rotation matrix math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); _att_sp.timestamp = hrt_absolute_time(); } else { reset_yaw_sp = true; } /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, * in this case the attitude setpoint is published by the mavlink app */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; } warnx("stopped"); mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); }
void MulticopterPositionControl::control_manual(float dt) { _sp_move_rate.zero(); if (_control_mode.flag_control_altitude_enabled) { if(_reset_mission) { _reset_mission = false; _mode_mission = 1 ; _hover_time = 0.0 ; } float height_hover_constant=-1.0; float hover_time_constant = 20.0; switch(_mode_mission) { case 1: _sp_move_rate(2) = -0.8; if(_pos_sp(2)<=height_hover_constant) _mode_mission=2; break; case 2: _hover_time += dt; if(_hover_time>hover_time_constant) { _hover_time=0.0; _mode_mission=3; } break; case 3: _pos_sp_triplet.current.type =position_setpoint_s::SETPOINT_TYPE_LAND; break; default: /* move altitude setpoint with throttle stick */ _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); break; } } if (_control_mode.flag_control_position_enabled) { /* move position setpoint with roll/pitch stick */ _sp_move_rate(0) = _manual.x; _sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ float sp_move_norm = _sp_move_rate.length(); if (sp_move_norm > 1.0f) { _sp_move_rate /= sp_move_norm; } /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */ reset_pos_sp(); } /* feed forward setpoint move rate with weight vel_ff */ _vel_ff = _sp_move_rate.emult(_params.vel_ff); /* move position setpoint */ _pos_sp += _sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } }
void MulticopterPositionControl::control_manual(float dt) { _sp_move_rate.zero(); if (_control_mode.flag_control_altitude_enabled) { /* move altitude setpoint with throttle stick */ _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { /* move position setpoint with roll/pitch stick */ _sp_move_rate(0) = _manual.x; _sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ float sp_move_norm = _sp_move_rate.length(); if (sp_move_norm > 1.0f) { _sp_move_rate /= sp_move_norm; } /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */ reset_pos_sp(); } /* feed forward setpoint move rate with weight vel_ff */ _vel_ff = _sp_move_rate.emult(_params.vel_ff); /* move position setpoint */ _pos_sp += _sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } }
void run () { Image::Header H (argument[0]); Image::Info info (H); info.set_ndim (3); Image::BufferScratch<bool> mask (info); auto v_mask = mask.voxel(); std::string mask_path; Options opt = get_options ("mask"); if (opt.size()) { mask_path = std::string(opt[0][0]); Image::Buffer<bool> in (mask_path); if (!Image::dimensions_match (H, in, 0, 3)) throw Exception ("Input mask image does not match DWI"); if (!(in.ndim() == 3 || (in.ndim() == 4 && in.dim(3) == 1))) throw Exception ("Input mask image must be a 3D image"); auto v_in = in.voxel(); Image::copy (v_in, v_mask, 0, 3); } else { for (auto l = Image::LoopInOrder (v_mask) (v_mask); l; ++l) v_mask.value() = true; } DWI::CSDeconv<float>::Shared shared (H); const size_t lmax = DWI::lmax_for_directions (shared.DW_dirs); if (lmax < 4) throw Exception ("Cannot run dwi2response with lmax less than 4"); shared.lmax = lmax; Image::BufferPreload<float> dwi (H, Image::Stride::contiguous_along_axis (3)); DWI::Directions::Set directions (1281); Math::Vector<float> response (lmax/2+1); response.zero(); { // Initialise response function // Use lmax = 2, get the DWI intensity mean and standard deviation within the mask and // use these as the first two coefficients auto v_dwi = dwi.voxel(); double sum = 0.0, sq_sum = 0.0; size_t count = 0; Image::LoopInOrder loop (dwi, "initialising response function... ", 0, 3); for (auto l = loop (v_dwi, v_mask); l; ++l) { if (v_mask.value()) { for (size_t volume_index = 0; volume_index != shared.dwis.size(); ++volume_index) { v_dwi[3] = shared.dwis[volume_index]; const float value = v_dwi.value(); sum += value; sq_sum += Math::pow2 (value); ++count; } } } response[0] = sum / double (count); response[1] = - 0.5 * std::sqrt ((sq_sum / double(count)) - Math::pow2 (response[0])); // Account for scaling in SH basis response *= std::sqrt (4.0 * Math::pi); } INFO ("Initial response function is [" + str(response, 2) + "]"); // Algorithm termination options opt = get_options ("max_iters"); const size_t max_iters = opt.size() ? int(opt[0][0]) : DWI2RESPONSE_DEFAULT_MAX_ITERS; opt = get_options ("max_change"); const float max_change = 0.01 * (opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_MAX_CHANGE); // Should all voxels (potentially within a user-specified mask) be tested at every iteration? opt = get_options ("test_all"); const bool reset_mask = opt.size(); // Single-fibre voxel selection options opt = get_options ("volume_ratio"); const float volume_ratio = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_VOLUME_RATIO; opt = get_options ("dispersion_multiplier"); const float dispersion_multiplier = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_DISPERSION_MULTIPLIER; opt = get_options ("integral_multiplier"); const float integral_multiplier = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_INTEGRAL_STDEV_MULTIPLIER; SFThresholds thresholds (volume_ratio); // Only threshold the lobe volume ratio for now; other two are not yet used size_t total_iter = 0; bool first_pass = true; size_t prev_sf_count = 0; { bool iterate = true; size_t iter = 0; ProgressBar progress ("optimising response function... "); do { ++iter; { MR::LogLevelLatch latch (0); shared.set_response (response); shared.init(); } ++progress; if (reset_mask) { if (mask_path.size()) { Image::Buffer<bool> in (mask_path); auto v_in = in.voxel(); Image::copy (v_in, v_mask, 0, 3); } else { for (auto l = Image::LoopInOrder(v_mask) (v_mask); l; ++l) v_mask.value() = true; } ++progress; } std::vector<FODSegResult> seg_results; { FODCalcAndSeg processor (dwi, mask, shared, directions, lmax, seg_results); Image::ThreadedLoop loop (mask, 0, 3); loop.run (processor); } ++progress; if (!first_pass) thresholds.update (seg_results, dispersion_multiplier, integral_multiplier, iter); ++progress; Response output (lmax); mask.zero(); { SFSelector selector (seg_results, thresholds, mask); ResponseEstimator estimator (dwi, shared, lmax, output); Thread::run_queue (selector, FODSegResult(), Thread::multi (estimator)); } if (!output.get_count()) throw Exception ("Cannot estimate response function; all voxels have been excluded from selection"); const Math::Vector<float> new_response = output.result(); const size_t sf_count = output.get_count(); ++progress; if (App::log_level >= 2) std::cerr << "\n"; INFO ("Iteration " + str(iter) + ", " + str(sf_count) + " SF voxels, new response function: [" + str(new_response, 2) + "]"); if (sf_count == prev_sf_count) { INFO ("terminating due to convergence of single-fibre voxel selection"); iterate = false; } if (iter == max_iters) { INFO ("terminating due to completing maximum number of iterations"); iterate = false; } bool rf_changed = false; for (size_t i = 0; i != response.size(); ++i) { if (std::abs ((new_response[i] - response[i]) / new_response[i]) > max_change) rf_changed = true; } if (!rf_changed) { INFO ("terminating due to negligible changes in the response function coefficients"); iterate = false; } if (!iterate && first_pass) { iterate = true; first_pass = false; INFO ("commencing second-pass of response function estimation"); total_iter = iter; iter = 0; } response = new_response; prev_sf_count = sf_count; //v_mask.save ("mask_pass_" + str(first_pass?1:2) + "_iter_" + str(iter) + ".mif"); } while (iterate); total_iter += iter; } CONSOLE ("final response function: [" + str(response, 2) + "] (reached after " + str(total_iter) + " iterations using " + str(prev_sf_count) + " voxels)"); response.save (argument[1]); opt = get_options ("sf"); if (opt.size()) v_mask.save (std::string (opt[0][0])); }
/* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } /* get transformation matrix from sensor/board to body frame */ get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation); /* fine tune the rotation */ math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0], M_DEG_TO_RAD_F * _params.board_offset[1], M_DEG_TO_RAD_F * _params.board_offset[2]); _board_rotation = board_rotation_offset * _board_rotation; // get the raw gyro data and correct for thermal errors math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0], _sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1], _sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]); // rotate corrected measurements from sensor to body frame rates = _board_rotation * rates; // correct for in-run bias errors rates(0) -= _ctrl_state.roll_rate_bias; rates(1) -= _ctrl_state.pitch_rate_bias; rates(2) -= _ctrl_state.yaw_rate_bias; math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p)); math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i)); math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d)); /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; _att_control = rates_p_scaled.emult(rates_err) + _rates_int + rates_d_scaled.emult(_rates_prev - rates) / dt + _params.rate_ff.emult(_rates_sp); _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if motors are providing enough thrust to be effective */ if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { // Check for positive control saturation bool positive_saturation = ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) || ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) || ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos); // Check for negative control saturation bool negative_saturation = ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) || ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) || ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg); // prevent further positive control saturation if (positive_saturation) { rates_err(i) = math::min(rates_err(i), 0.0f); } // prevent further negative control saturation if (negative_saturation) { rates_err(i) = math::max(rates_err(i), 0.0f); } // Perform the integration using a first order method and do not propaate the result if out of range or invalid float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) { _rates_int(i) = rate_i; } } } /* explicitly limit the integrator state */ for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { _rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i)); } }
void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT); for (unsigned s = 0; s < _gyro_count; s++) { _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); } _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); /* initialize parameters cache */ parameters_update(); /* wakeup source: gyro data from sensor selected by the sensor app */ px4_pollfd_struct_t poll_fds = {}; poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; poll_fds.events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = px4_poll(&poll_fds, 1, 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on gyro changes */ if (poll_fds.revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy gyro data */ orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); battery_status_poll(); control_state_poll(); sensor_correction_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { // the tailsitter recovery instance has not been created, thus, the vehicle // is not a tailsitter, do normal attitude control control_attitude(dt); } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */ for (int i = 0; i < 3; i++) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = _v_att_sp.landing_gear; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; /* scale effort by battery status */ if (_params.bat_scale_en && _battery_status.scale > 0.0f) { for (int i = 0; i < 4; i++) { _actuators.control[i] *= _battery_status.scale; } } _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); /* publish actuator controls */ _actuators.control[0] = 0.0f; _actuators.control[1] = 0.0f; _actuators.control[2] = 0.0f; _actuators.control[3] = 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } } } perf_end(_loop_perf); } _control_task = -1; return; }