void run() {
Image::Header input_SH_header (argument[0]);
if (input_SH_header.ndim() != 4)
throw Exception ("input SH image should contain 4 dimensions");
std::vector<ssize_t> strides (4, 0);
strides[3] = 1;
Image::BufferPreload<value_type> input_buf (input_SH_header, strides);
Math::Vector<value_type> responseSH;
responseSH.load (argument[1]);
Math::Vector<value_type> responseRH;
Math::SH::SH2RH (responseRH, responseSH);
Ptr<Image::Buffer<bool> > mask_buf;
Options opt = get_options ("mask");
if (opt.size()) {
mask_buf = new Image::Buffer<bool> (opt[0][0]);
Image::check_dimensions (*mask_buf, input_buf, 0, 3);
}
Image::Header output_SH_header (input_SH_header);
Image::Stride::set_from_command_line (output_SH_header, Image::Stride::contiguous_along_axis (3));
Image::Buffer<value_type> output_SH_buf (argument[2], output_SH_header);
SDeconvFunctor sconv (input_buf, output_SH_buf, mask_buf, responseRH);
Image::ThreadedLoop loop ("performing convolution...", input_buf, 2, 0, 3);
loop.run (sconv);
}
/*! \brief A ray-rod intersection test.
A rod is a cylinder which is not infinite, but of limited
length. The cylinder is defined using a single base vertex at
the center of the bottom circular face and an axial vector
pointing from the base vertex to the top vertex. This test
ignores the back face of the rod. It is used to detect when a
ray will enter a rod.
\param T The origin of the ray relative to the base vertex.
\param D The direction/velocity of the ray.
\param A The axial vector of the rod.
\param r Radius of the rod.
\return The time until the intersection, or HUGE_VAL if no intersection.
*/
inline double ray_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
{
double t = ray_cylinder(T, D, A / A.nrm(), r);
double Tproj = ((T + t * D) | A);
if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;
return t;
}
void ALSound::SetListener(const Math::Vector &eye, const Math::Vector &lookat)
{
m_eye = eye;
m_lookat = lookat;
Math::Vector forward = lookat - eye;
forward.Normalize();
float orientation[] = {forward.x, forward.y, forward.z, 0.f, -1.0f, 0.0f};
alListener3f(AL_POSITION, eye.x, eye.y, eye.z);
alListenerfv(AL_ORIENTATION, orientation);
}
/*! \brief A ray-inverse_rod intersection test.
A rod is a cylinder which is not infinite, but of limited
length. An inverse rod is used to test when a ray will exit a
rod. The cylinder is defined using a single base vertex at the
center of the bottom circular face and an axial vector
pointing from the base vertex to the top vertex. This test
ignores the back face of the rod.
\param T The origin of the ray relative to the base vertex.
\param D The direction/velocity of the ray.
\param A The axial vector of the inverse rod.
\param r Radius of the inverse rod.
\tparam always_intersect If true, this will ensure that glancing
ray's never escape the enclosing sphere by returning the time
when the ray is nearest the sphere if the ray does not intersect
the sphere.
\return The time until the intersection, or HUGE_VAL if no intersection.
*/
inline double ray_inv_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
{
double t = ray_inv_cylinder(T, D, A / A.nrm(), r);
M_throw() << "Confirm that this function is correct";
double Tproj = ((T + t * D) | A);
if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;
return t;
}
//! \brief A ray-sphere intersection test with backface culling.
//!
//! \param T The origin of the ray relative to the sphere center.
//! \param D The direction/velocity of the ray.
//! \param r The radius of the sphere.
//! \return The time until the intersection, or HUGE_VAL if no intersection.
inline double ray_sphere_bfc(const math::Vector& T,
const math::Vector& D,
const double& r)
{
double TD = (T | D);
if (TD >= 0) return HUGE_VAL;
double c = T.nrm2() - r * r;
double arg = TD * TD - D.nrm2() * c;
if (arg < 0) return HUGE_VAL;
return - c / (TD - std::sqrt(arg));
}
//! \brief A ray-inverse_sphere intersection test with backface culling.
//!
//! An inverse sphere means an "enclosing" sphere.
//!
//! \param T The origin of the ray relative to the inverse sphere
//! center.
//! \param D The direction/velocity of the ray.
//! \param d The diameter of the inverse sphere.
//! \return The time until the intersection, or HUGE_VAL if no intersection.
inline double ray_inv_sphere_bfc(const math::Vector& T,
const math::Vector& D,
const double& r)
{
double D2 = D.nrm2();
if (D2 == 0) return HUGE_VAL;
double TD = T | D;
double arg = TD * TD - D2 * (T.nrm2() - r * r);
if (arg < 0) return HUGE_VAL;
return (std::sqrt(arg) - TD) / D2;
}
/*
* 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;
/* 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;
}
}
}
}
}
void math::Matrix::assignToBucket(math::Vector &vector) {
math::Space *space = vector.getSpace();
int column = std::floor(vector.get(math::Dimension::X) / bucketSize[math::Dimension::X]);
int row = std::floor(vector.get(math::Dimension::Y) / bucketSize[math::Dimension::Y]);
int dimension = std::floor(std::min<double>(vector.get(math::Dimension::Z), Vector::getSpace()->getMax(math::Dimension::Z)) /
bucketSize[math::Dimension::Z]);
if (column == space->getMax(math::Dimension::X) / bucketSize[math::Dimension::X])
column--;
if (row == space->getMax(math::Dimension::Y) / bucketSize[math::Dimension::Y])
row--;
if (dimension == space->getMax(math::Dimension::Z) / bucketSize[math::Dimension::Z])
dimension--;
values[row][column][dimension]++;
}
template<UnsignedInt dimensions> std::size_t AbstractImage::dataSize(Math::Vector<dimensions, Int> size) const {
/** @todo Code this properly when all @fn_gl{PixelStore} parameters are implemented */
/* Row size, rounded to multiple of 4 bytes */
const std::size_t rowSize = ((size[0]*pixelSize() + 3)/4)*4;
/** @todo Can't this be done somewhat nicer? */
size[0] = 1;
return rowSize*size.product();
}
void
MulticopterPositionControl::control_offboard(float dt)
{
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
}
if (_pos_sp_triplet.current.yaw_valid) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
/* Control altitude */
_pos_sp(2) = _pos_sp_triplet.current.z;
} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
}
/* 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;
} else {
reset_pos_sp();
reset_alt_sp();
}
}
OffcentreSpheresOverlapFunction(const math::Vector& rij, const math::Vector& vij, const math::Vector& omegai, const math::Vector& omegaj,
const math::Vector& nu1, const math::Vector& nu2, const double diameter1, const double diameter2,
const double maxdist, const double t, const double invgamma, const double t_min, const double t_max):
w1(omegai), w2(omegaj), u1(nu1), u2(nu2), r12(rij), v12(vij), _diameter1(diameter1), _diameter2(diameter2), _invgamma(invgamma), _t(t), _t_min(t_min), _t_max(t_max)
{
double Gmax = std::max(1 + t * invgamma, 1 + (t + t_max) * invgamma);
const double sigmaij = 0.5 * (_diameter1 + _diameter2);
const double sigmaij2 = sigmaij * sigmaij;
double magw1 = w1.nrm(), magw2 = w2.nrm();
double rijmax = Gmax * std::max(maxdist, rij.nrm());
#ifdef MAGNET_DEBUG
if (rij.nrm() > 1.0001 * maxdist)
std::cout << "WARNING!: Particle separation is larger than the maximum specified. " <<rij.nrm() << ">" << maxdist << "\n";
#endif
double magu1 = u1.nrm(), magu2 = u2.nrm();
double vijmax = v12.nrm() + Gmax * (magu1 * magw1 + magu2 * magw2) + std::abs(invgamma) * (magu1 + magu2);
double aijmax = Gmax * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2) + 2 * std::abs(invgamma) * (magu1 * magw1 + magu2 * magw2);
double dotaijmax = Gmax * (magu1 * magw1 * magw1 * magw1 + magu2 * magw2 * magw2 * magw2) + 3 * std::abs(invgamma) * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2);
_f1max = 2 * rijmax * vijmax + 2 * Gmax * std::abs(invgamma) * sigmaij2;
_f2max = 2 * vijmax * vijmax + 2 * rijmax * aijmax + 2 * invgamma * invgamma * sigmaij2;
_f3max = 6 * vijmax * aijmax + 2 * rijmax * dotaijmax;
}
std::array<double, nderivs> eval(const double dt = 0) const
{
math::Vector u1new = Rodrigues(w1 * dt) * math::Vector(u1);
math::Vector u2new = Rodrigues(w2 * dt) * math::Vector(u2);
const double colldiam = 0.5 * (_diameter1 + _diameter2);
const double growthfactor = 1 + _invgamma * (_t + dt);
const math::Vector rij = r12 + dt * v12 + growthfactor * (u1new - u2new);
const math::Vector vij = v12 + growthfactor * ((w1 ^ u1new) - (w2 ^ u2new)) + _invgamma * (u1new - u2new);
const math::Vector aij = growthfactor * (-w1.nrm2() * u1new + w2.nrm2() * u2new) + 2 * _invgamma * ((w1 ^ u1new) - (w2 ^ u2new));
const math::Vector dotaij = growthfactor * (-w1.nrm2() * (w1 ^ u1new) + w2.nrm2() * (w2 ^ u2new)) + 3 * _invgamma * (-w1.nrm2() * u1new + w2.nrm2() * u2new);
std::array<double, nderivs> retval;
for (size_t i(0); i < nderivs; ++i)
switch (first_deriv + i) {
case 0: retval[i] = (rij | rij) - growthfactor * growthfactor * colldiam * colldiam; break;
case 1: retval[i] = 2 * (rij | vij) - 2 * _invgamma * growthfactor * colldiam * colldiam; break;
case 2: retval[i] = 2 * vij.nrm2() + 2 * (rij | aij) - 2 * _invgamma * _invgamma * colldiam * colldiam; break;
case 3: retval[i] = 6 * (vij | aij) + 2 * (rij | dotaij); break;
default:
M_throw() << "Invalid access";
}
return retval;
}
/*
* 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::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::task_main()
{
warnx("started");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mpc] started");
/*
* 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));
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 was_armed = false;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
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;
}
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;
sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* 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) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* 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;
}
/* scale 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);
/* 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);
}
} else {
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_pos_sp();
reset_alt_sp();
}
}
/* fill local position setpoint */
_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;
/* 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);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][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) + sp_move_rate.emult(_params.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 == SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in non-manual modes */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
if (vel_sp_norm > 1.0f) {
_vel_sp /= vel_sp_norm;
}
}
_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 == 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 / 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 (_att.R[2][2] > TILT_COS_MAX) {
att_comp = 1.0f / _att.R[2][2];
} else if (_att.R[2][2] > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _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][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][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;
_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 {
reset_int_z = true;
}
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
/* 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);
}
//==================================================
//! Dot Product
//==================================================
float Math::Vector::operator*( const Math::Vector& rhs ) const {
return x*rhs.X() + y*rhs.Y() + z*rhs.Z();
}
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;
}
void MulticopterPositionControl::control_auto(float dt)
{
if (!_mode_auto) {
_mode_auto = true;
/* reset position setpoint on AUTO mode activation */
reset_pos_sp();
reset_alt_sp();
}
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid
if (!isfinite(_pos_sp_triplet.current.lat) ||
!isfinite(_pos_sp_triplet.current.lon) ||
!isfinite(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
/* find X - cross point of L1 sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than L1 radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
if (_pos_sp_triplet.next.valid) {
math::Vector<3> next_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
&next_sp.data[0], &next_sp.data[1]);
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
if ((next_sp - curr_sp).length() > MIN_DIST) {
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* cos(b), b = angle pos - curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than L1 radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
}
}
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (near) {
/* L1 sphere crosses trajectory */
} else {
/* copter is too far from trajectory */
/* if copter is behind prev waypoint, go directly to prev waypoint */
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
pos_sp_s = prev_sp_s;
}
/* if copter is in front of curr waypoint, go directly to curr waypoint */
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
}
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
}
}
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
}
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 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
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 () {
size_t niter = 10000;
float target_power = 2.0;
Options opt = get_options ("power");
if (opt.size())
target_power = opt[0][0];
opt = get_options ("niter");
if (opt.size())
niter = opt[0][0];
ndirs = to<int> (argument[0]);
if (get_options ("unipolar").size())
bipolar = false;
Math::RNG rng;
Math::Vector<double> v (2*ndirs);
for (size_t n = 0; n < 2*ndirs; n+=2) {
v[n] = Math::pi * (2.0 * rng.uniform() - 1.0);
v[n+1] = std::asin (2.0 * rng.uniform() - 1.0);
}
gsl_multimin_function_fdf fdf;
fdf.f = energy_f;
fdf.df = energy_df;
fdf.fdf = energy_fdf;
fdf.n = 2*ndirs;
gsl_multimin_fdfminimizer* minimizer =
gsl_multimin_fdfminimizer_alloc (gsl_multimin_fdfminimizer_conjugate_fr, 2*ndirs);
{
ProgressBar progress ("Optimising directions...");
for (power = -1.0; power >= -target_power/2.0; power *= 2.0) {
INFO ("setting power = " + str (-power*2.0));
gsl_multimin_fdfminimizer_set (minimizer, &fdf, v.gsl(), 0.01, 1e-4);
for (size_t iter = 0; iter < niter; iter++) {
int status = gsl_multimin_fdfminimizer_iterate (minimizer);
//for (size_t n = 0; n < 2*ndirs; ++n)
//std::cout << gsl_vector_get (minimizer->x, n) << " " << gsl_vector_get (minimizer->gradient, n) << "\n";
if (iter%10 == 0)
INFO ("[ " + str (iter) + " ] (pow = " + str (-power*2.0) + ") E = " + str (minimizer->f)
+ ", grad = " + str (gsl_blas_dnrm2 (minimizer->gradient)));
if (status) {
INFO (std::string ("iteration stopped: ") + gsl_strerror (status));
break;
}
progress.update ([&]() { return "Optimising directions (power " + str(-2.0*power) + ", current energy: " + str(minimizer->f, 8) + ")..."; });
}
gsl_vector_memcpy (v.gsl(), minimizer->x);
}
}
Math::Matrix<double> directions (ndirs, 2);
for (size_t n = 0; n < ndirs; n++) {
double az = gsl_vector_get (minimizer->x, 2*n);
double el = gsl_vector_get (minimizer->x, 2*n+1);
range (az, el);
directions (n, 0) = az;
directions (n, 1) = el;
}
gsl_multimin_fdfminimizer_free (minimizer);
DWI::Directions::save (directions, argument[1], get_options ("cartesian").size());
}
Normal::Normal(const math::Vector &vec) : mImpl(new Impl) {
mImpl->vec.x = vec.x();
mImpl->vec.y = vec.y();
mImpl->vec.z = vec.z();
}
void ScreenCapture::on_screen_capture ()
{
if (isnan (rotation_axis_x->value()))
rotation_axis_x->setValue (0.0);
if (isnan (rotation_axis_y->value()))
rotation_axis_y->setValue (0.0);
if (isnan (rotation_axis_z->value()))
rotation_axis_z->setValue (0.0);
if (isnan (degrees_button->value()))
degrees_button->setValue (0.0);
if (isnan (translate_x->value()))
translate_x->setValue(0.0);
if (isnan (translate_y->value()))
translate_y->setValue(0.0);
if (isnan (translate_z->value()))
translate_z->setValue(0.0);
if (isnan (FOV_multipler->value()))
FOV_multipler->setValue(1.0);
if (window.snap_to_image () && degrees_button->value() > 0.0)
window.set_snap_to_image (false);
float radians = degrees_button->value() * (M_PI / 180.0) / frames->value();
std::string folder (directory->path().toUtf8().constData());
std::string prefix (prefix_textbox->text().toUtf8().constData());
int first_index = start_index->value();
int i = first_index;
for (; i < first_index + frames->value(); ++i) {
this->window.captureGL (folder + "/" + prefix + printf ("%04d.png", i));
// Rotation
Math::Versor<float> orientation (this->window.orientation());
Math::Vector<float> axis (3);
axis[0] = rotation_axis_x->value();
axis[1] = rotation_axis_y->value();
axis[2] = rotation_axis_z->value();
Math::Versor<float> rotation (radians, axis.ptr());
rotation *= orientation;
this->window.set_orientation (rotation);
// Translation
Point<float> focus (this->window.focus());
focus[0] += translate_x->value() / frames->value();
focus[1] += translate_y->value() / frames->value();
focus[2] += translate_z->value() / frames->value();
window.set_focus (focus);
Point<float> target (this->window.target());
target[0] += translate_x->value() / frames->value();
target[1] += translate_y->value() / frames->value();
target[2] += translate_z->value() / frames->value();
window.set_target (target);
// FOV
window.set_FOV (window.FOV() * (Math::pow (FOV_multipler->value(), (float) 1.0 / frames->value())));
start_index->setValue (i + 1);
this->window.updateGL();
}
}
void run ()
{
Image::Buffer<value_type> dwi_data (argument[0]);
if (dwi_data.ndim() != 4)
throw Exception ("dwi image should contain 4 dimensions");
Math::Matrix<value_type> grad = DWI::get_valid_DW_scheme<value_type> (dwi_data);
DWI::Shells shells (grad);
// Keep the b=0 shell (may be used for normalisation), but force single non-zero shell
shells.select_shells (true, true);
Math::Matrix<value_type> DW_dirs = DWI::gen_direction_matrix (grad, shells.largest().get_volumes());
Options opt = get_options ("lmax");
lmax = opt.size() ? opt[0][0] : Math::SH::LforN (shells.largest().count());
INFO ("calculating even spherical harmonic components up to order " + str (lmax));
Math::Matrix<value_type> HR_dirs;
Math::Matrix<value_type> HR_SHT;
opt = get_options ("normalise");
if (opt.size()) {
normalise = true;
opt = get_options ("directions");
if (opt.size())
HR_dirs.load (opt[0][0]);
else
DWI::Directions::electrostatic_repulsion_300 (HR_dirs);
Math::SH::init_transform (HR_SHT, HR_dirs, lmax);
}
// set Lmax
int i;
for (i = 0; Math::SH::NforL(i) < shells.largest().count(); i += 2);
i -= 2;
if (lmax > i) {
WARN ("not enough data for SH order " + str(lmax) + ", falling back to " + str(i));
lmax = i;
}
INFO("setting maximum even spherical harmonic order to " + str(lmax));
// Setup response function
int num_RH = (lmax + 2)/2;
Math::Vector<value_type> sigs(num_RH);
std::vector<value_type> AL (lmax+1);
Math::Legendre::Plm_sph<value_type>(&AL[0], lmax, 0, 0);
for (int l = 0; l <= lmax; l += 2) sigs[l/2] = AL[l];
Math::Vector<value_type> response(num_RH);
Math::SH::SH2RH(response, sigs);
opt = get_options ("filter");
Math::Vector<value_type> filter;
if (opt.size()) {
filter.load (opt[0][0]);
if (filter.size() <= response.size())
throw Exception ("not enough filter coefficients supplied for lmax" + str(lmax));
for (int i = 0; i <= lmax/2; i++) response[i] *= filter[i];
INFO ("using initial filter coefficients: " + str (filter));
}
Math::SH::Transform<value_type> FRT_SHT(DW_dirs, lmax);
FRT_SHT.set_filter(response);
Image::Header qbi_header (dwi_data);
qbi_header.dim(3) = Math::SH::NforL (lmax);
qbi_header.datatype() = DataType::Float32;
Image::Stride::set (qbi_header, Image::Stride::contiguous_along_axis (3, dwi_data));
Image::Buffer<value_type> qbi_data (argument[1], qbi_header);
opt = get_options ("mask");
if (opt.size()) {
Image::Buffer<bool> mask_data (opt[0][0]);
Image::ThreadedLoop ("estimating dODFs using Q-ball imaging...", dwi_data, 0, 3)
.run (DWI2QBI (FRT_SHT.mat_A2SH(), HR_SHT, shells), mask_data.voxel(), dwi_data.voxel(), qbi_data.voxel());
}
else
Image::ThreadedLoop ("estimating dODFs using Q-ball imaging...", dwi_data, 0, 3)
.run (DWI2QBI (FRT_SHT.mat_A2SH(), HR_SHT, shells), dwi_data.voxel(), qbi_data.voxel());
}
