/**
* @brief Draw a line
*
* Draws a straight line between the two points.
*
* @param x1 the x coordinate of the first point
* @param y2 the y coordinate of the first point
* @param x1 the x coordinate of the second point
* @param y2 the y coordinate of the second point
* @param color the Color to of the line
*/
void CatPictureApp::drawLine(int x1, int y1, int x2, int y2, Color8u* color)
// changed the names of the parametets, thought x1 would make more sense
// to the user than xF, fairly arbitrary though.. might just be personal preference
{
//Implementation of Bresenham's line algorithm, derived from pseudocode
//from Wikipedia.
int dx = intMath->abs(x2 - x1);
int dy = intMath->abs(y2 - y1);
int sx = intMath->signum(x2 - x1);
int sy = intMath->signum(y2 - y1);
int err = dx - dy;
int x = x1;
int y = y1;
while(true)
{
modify(color,x,y);
if(x == x2 && y == y2)
break;
int e2 = 2 * err;
if(e2 > -dy)
{
err -= dy;
x += sx;
}
if(e2 < dx)
{
err += dx;
y += sy;
}
}
}
Tensor HeightField::operator()(math::Vector2f const & p) const
{
static const int dx = 2, dy = 2;
if (m_image.isNull())
{
return Tensor();
}
QPoint ip = m_image.toImageCoords(p).toPoint();
if (! QRect(QPoint(0,0), m_image.size()-QSize(dx,dy)).contains(ip))
{
return Tensor();
}
QRgb pix0 = m_image.pixel(ip);
float f0 = QColor::fromRgb(pix0).valueF();
QRgb pix1 = m_image.pixel(ip + QPoint(dx,0));
float f1 = QColor::fromRgb(pix1).valueF();
QRgb pix2 = m_image.pixel(ip + QPoint(0,dy));
float f2 = QColor::fromRgb(pix2).valueF();
qreal dHx = 100.0f * (f1 - f0);
qreal dHy = 100.0f * (f2 - f0);
qreal f = atan2f(dHy, dHx) + M_PI/2.0f;
qreal r = sqrtf(pow2(dHx) + pow2(dHy));
return Tensor(r, f);
}
/**
* Draws a straight line between the two points.
* Params:
xI = Initial X
yI = Initial Y
xF = Final X
yF = Final Y
color = Color to draw.
*/
void CatPictureApp::drawLine(int xI, int yI, int xF, int yF, Color8u* color)
{
//Implementation of Bresenham's line algorithm, derived from pseudocode
//from Wikipedia.
int dx = intMath->abs(xF - xI);
int dy = intMath->abs(yF - yI);
int sx = intMath->signum(xF - xI);
int sy = intMath->signum(yF - yI);
int err = dx - dy;
int x = xI;
int y = yI;
while(true)
{
modify(color,x,y);
if(x == xF && y == yF)
break;
int e2 = 2 * err;
if(e2 > -dy)
{
err -= dy;
x += sx;
}
if(e2 < dx)
{
err += dx;
y += sy;
}
}
}
typename traits::unit_vector<Value>::type
inline unit_vector(std::size_t k, std::size_t n)
{
using math::zero; using math::one;
dense_vector<Value> v(n, zero(Value()));
v[k]= one(Value());
return v;
}
Scalar getAbsoluteTimeInSeconds() const
{
// This is super wonky, should be improved someday
const auto delta = chVTTimeElapsedSinceX(previous_time_systicks_);
previous_time_systicks_ += delta;
absolute_time_systicks_ += delta;
return Scalar(absolute_time_systicks_) / Scalar(CH_CFG_ST_FREQUENCY);
}
geom::aabb Object::getBounds() const {
geom::aabb bounds(
vec3f(
vertices_[0], vertices_[1], vertices_[2]));
for(int i=1; i<nVertices_; ++i) {
bounds.extendToFit(
vec3f(
vertices_[3*i], vertices_[3*i+1], vertices_[3*i+2]));
}
return bounds;
}
math::Tensor BasisField::operator()(math::Vector2f const & p) const
{
if (0 == m_singularityType)
{
return scale * m_regularValue;
}
else
{
Q_ASSERT(m_singularityType < NumSingularityTypes);
Vector2f v = (p - p0).normalized();
float x = v(0);
float y = v(1);
switch (m_singularityType)
{
case SingularityType_Center:
return scale * math::Tensor::fromValues(pow2(y)-pow2(x), -2.0f*x*y);
case SingularityType_Wedge:
return scale * math::Tensor::fromValues(x, y);
case SingularityType_Node:
return scale * math::Tensor::fromValues(pow2(x)-pow2(y), 2.0f*x*y);
case SingularityType_Trisector:
return scale * math::Tensor::fromValues(x, -y);
case SingularityType_Saddle:
return scale * math::Tensor::fromValues(pow2(x)-pow2(y), -2.0f*x*y);
case SingularityType_Focus:
return scale * math::Tensor::fromValues(pow2(y)-pow2(x), 2.0f*x*y);
default:
// not reached, hopefully
return math::Tensor();
}
}
}
void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
{
// Calculate the time in seconds since the last update and use the default time step value if out of bounds
uint64_t now = ecl_absolute_time();
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
// Convert equivalent airspeed quantities to true airspeed
_EAS_setpoint = airspeed_setpoint;
_TAS_setpoint = _EAS_setpoint * EAS2TAS;
_TAS_max = _indicated_airspeed_max * EAS2TAS;
_TAS_min = _indicated_airspeed_min * EAS2TAS;
// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
// min and max limits
if (!ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
} else {
_EAS = indicated_airspeed;
}
// If first time through or not flying, reset airspeed states
if (_speed_update_timestamp == 0 || !_in_air) {
_tas_rate_state = 0.0f;
_tas_state = (_EAS * EAS2TAS);
}
// Obtain a smoothed airspeed estimate using a second order complementary filter
// Update TAS rate state
float tas_error = (_EAS * EAS2TAS) - _tas_state;
float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;
// limit integrator input to prevent windup
if (_tas_state < 3.1f) {
tas_rate_state_input = max(tas_rate_state_input, 0.0f);
}
// Update TAS state
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
_tas_state = _tas_state + tas_state_input * dt;
// Limit the airspeed state to a minimum of 3 m/s
_tas_state = max(_tas_state, 3.0f);
_speed_update_timestamp = now;
}
static void setVariableFromIRQ(const Value value)
{
static_assert(Index < NumVariables, "Index out of range");
auto& self = getInstance();
self.vars_[Index] = Scalar(value);
self.update_flags_[Index] = true;
}
dense2D<typename mtl::Collection
<typename mtl::Collection<VVector>::value_type
>::value_type, parameters<> >
inline orthogonalize_factors(VVector& v, tag::vector)
{
using ::mtl::two_norm;
using math::zero;
using mtl::size1D;
typedef typename mtl::Collection<VVector>::size_type Size;
typedef typename mtl::Collection<VVector>::value_type Vector;
typedef typename mtl::Collection<Vector>::value_type Scalar;
dense2D<Scalar, parameters<> > tau(size1D(v), size1D(v));
tau= zero(Scalar());
for (Size j= 0; j < size1D(v); ++j) {
for (Size i= 0; i < j; ++i) {
Scalar t= dot(entry1D(v, i), entry1D(v, j)) / tau[i][i];
tau[i][j]= t;
entry1D(v, j)-= t * entry1D(v, i);
}
tau[j][j]= dot(entry1D(v, j), entry1D(v, j));
}
return tau;
}
Tensor BasisSumField::operator()(Vector2f const & p) const
{
ElementList::const_iterator it;
unsigned int i;
unsigned int numElements = m_elements.size();
if (numElements == 0)
{
return Tensor();
}
typedef float real;
typedef real realv[numElements];
// calculate distances
//
realv dists;
real distsSum = 0.0f;
for (i = 0, it = m_elements.begin(); i < numElements; ++i, ++it)
{
Element * bf = *it;
real d = (p - bf->p0).norm();
if (d == 0.0) d = 0.001;
dists[i] = d;
distsSum += d;
}
// calculate "nearness" value
//
realv nears;
real nearsSum = 0.0f;
for (i = 0; i < numElements; ++i)
{
real n = math::pow2(distsSum / dists[i]);
nears[i] = n;
nearsSum += n;
}
// calculate vector sum
//
math::Tensor t;
for (i = 0, it = m_elements.begin(); i < numElements; ++i, ++it)
{
Element * bf = *it;
float w = nears[i] / nearsSum;
t += w * rbf(p, bf->p0, decay()) * bf->scale * (*bf)(p);
}
return t;
}
void _gemm_x8s8s32x_convolution_fwd_t<src_type, dst_type>::pp_ker_t::operator ()
(dst_data_t *dst, const acc_data_t *acc, const char *bias,
const float *scales, float nslope, float sum_scale, float signed_scale,
int g, size_t start, size_t end)
{
using math::get_bias;
if (end <= start)
return;
if (ker_) {
// JIT
ker_args args;
size_t oc_offset = start % OC_;
size_t os_offset = start / OC_;
args.acc = acc + start;
args.dst = dst + os_offset * dst_os_stride_ + oc_offset;
args.bias = bias + (g * jcp_.oc + oc_offset) * bias_data_type_size_;
args.scales = scales + scale_idx_mult_ * (g * jcp_.oc + oc_offset);
args.nslope = nslope;
args.sum_scale = sum_scale;
args.signed_scale = signed_scale;
args.len = end - start;
args.oc_offset = oc_offset;
ker_(&args);
}
else {
// Fallback
const size_t first_oc = start % OC_;
const size_t last_oc = (end - 1) % OC_;
const size_t first_os = start / OC_;
const size_t last_os = (end - 1) / OC_;
for (size_t os = first_os; os <= last_os; os++) {
const size_t start_oc = (os == first_os) ? first_oc : 0;
const size_t end_oc = (os == last_os) ? last_oc : OC_ - 1;
for (size_t oc = start_oc; oc <= end_oc; oc++) {
const size_t acc_off = os * jcp_.oc + oc;
const size_t dst_off = os * dst_os_stride_ + oc;
float d = (float)(acc[acc_off]);
if (jcp_.signed_input)
d *= signed_scale;
if (do_bias_)
d += get_bias(bias, g * jcp_.oc + oc,
bias_data_type_);
d *= scales[(g * jcp_.oc + oc) * scale_idx_mult_];
if (do_sum_)
d += sum_scale * dst[dst_off];
if (do_relu_ && d < 0)
d *= nslope;
dst[dst_off] = qz_a1b0<float, dst_data_t>()(d);
}
}
}
};
/// Constructor takes matrix reference
explicit diagonal(const Matrix& A) : inv_diag(num_rows(A))
{
mtl::vampir_trace<5050> tracer;
MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
using math::reciprocal;
for (size_type i= 0; i < num_rows(A); ++i)
inv_diag[i]= reciprocal(A[i][i]);
}
// To prevent that cout << A * B prints the element-wise product, suggestion by Hui Li
// It is rather inefficient, esp. for multiple products (complexity increases with the number of arguments :-!)
// or sparse matrices.
// Better compute your product first and print it then when compute time is an issue,
// this is ONLY for convenience.
result_value_type
operator()(std::size_t r, std::size_t c) const
{
using math::zero;
MTL_THROW_IF(num_cols(first) != num_rows(second), incompatible_size());
result_value_type ref, sum(zero(ref));
for (std::size_t i= 0; i < num_cols(first); i++)
sum+= first(r, i) * second(i, c);
return sum;
}
void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
float EAS2TAS)
{
if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initalized) {
// On first time through or when not using TECS of if there has been a large time slip,
// states must be reset to allow filters to a clean start
_vert_accel_state = 0.0f;
_vert_vel_state = 0.0f;
_vert_pos_state = baro_altitude;
_tas_rate_state = 0.0f;
_tas_state = _EAS * EAS2TAS;
_throttle_integ_state = 0.0f;
_pitch_integ_state = 0.0f;
_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
_pitch_setpoint_unc = _last_pitch_setpoint;
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _TAS_setpoint_last;
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
_STE_rate_error = 0.0f;
if (_dt > DT_MAX || _dt < DT_MIN) {
_dt = DT_DEFAULT;
}
} else if (_climbout_mode_active) {
// During climbout use the lower pitch angle limit specified by the
// calling controller
_pitch_setpoint_min = pitch_min_climbout;
// throttle lower limit is set to a value that prevents throttle reduction
_throttle_setpoint_min = _throttle_setpoint_max - 0.01f;
// height demand and associated states are set to track the measured height
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
// airspeed demand states are set to track the measured airspeed
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _EAS * EAS2TAS;
// disable speed and decent error condition checks
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
}
_states_initalized = true;
}
void run(alps::ObservableSet& obs) {
++mcs;
// initialize cluster information
std::fill(fragments.begin(), fragments.end(), fragment_t());
// cluster generation
for (double t = r_time(); t < 1; t += r_time()) {
int s0 = lattice.num_sites() * uniform_01();
int s1 = lattice.num_sites() * uniform_01();
if (spins[s0] == spins[s1]) unify(fragments, s0, s1);
}
// assign cluster id & accumulate cluster properties
int nc = 0;
double mag2 = 0, mag4 = 0;
BOOST_FOREACH(fragment_t& f, fragments) {
if (f.is_root()) {
f.set_id(nc++);
double w = f.weight();
mag2 += power2(w);
mag4 += power4(w);
}
}
BOOST_FOREACH(fragment_t& f, fragments) f.set_id(cluster_id(fragments, f));
// flip spins
for (int c = 0; c < nc; ++c) flip[c] = (uniform_01() < 0.5);
for (int s = 0; s < lattice.num_sites(); ++s)
if (flip[fragments[s].id()]) spins[s] ^= 1;
double mu = 0;
for (int s = 0; s < lattice.num_sites(); ++s) mu += 2 * spins[s] - 1;
obs["Number of Clusters"] << (double)nc;
obs["Magnetization (unimproved)"] << mu;
obs["Magnetization^2 (unimproved)"] << power2(mu);
obs["Magnetization^4 (unimproved)"] << power4(mu);
obs["Magnetization^2"] << mag2;
obs["Magnetization^4"] << (3 * power2(mag2) - 2 * mag4);
}
void TECS::_update_speed_setpoint()
{
// Set the airspeed demand to the minimum value if an underspeed or
// or a uncontrolled descent condition exists to maximise climb rate
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
_TAS_setpoint = _TAS_min;
}
_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// Calculate limits for the demanded rate of change of speed based on physical performance limits
// with a 50% margin to allow the total energy controller to correct for errors.
float velRateMax = 0.5f * _STE_rate_max / _tas_state;
float velRateMin = 0.5f * _STE_rate_min / _tas_state;
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// calculate the demanded rate of change of speed proportional to speed error
// and apply performance limits
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax);
}
// Undefined if matrix is not symmetric
void factorize(const Matrix& A, mtl::tag::sparse)
{
using namespace mtl; using namespace mtl::tag; using mtl::traits::range_generator;
using math::reciprocal; using mtl::matrix::upper;
typedef typename range_generator<row, U_type>::type cur_type;
typedef typename range_generator<nz, cur_type>::type icur_type;
MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
U= upper(A); crop(U);
U_type L(lower(A)); // needed to find non-zeros in column
typename mtl::traits::col<U_type>::type col(U), col_l(L);
typename mtl::traits::value<U_type>::type value(U);
cur_type kc= begin<row>(U), kend= end<row>(U);
for (size_type k= 0; kc != kend; ++kc, ++k) {
icur_type ic= begin<nz>(kc), iend= end<nz>(kc);
MTL_DEBUG_THROW_IF(col(*ic) != k, mtl::missing_diagonal());
// U[k][k]= 1.0 / sqrt(U[k][k]);
value_type inv_dia= reciprocal(sqrt(value(*ic)));
value(*ic, inv_dia);
icur_type jbegin= ++ic;
for (; ic != iend; ++ic) {
// U[k][i] *= U[k][k]
value_type d= value(*ic) * inv_dia;
value(*ic, d);
size_type i= col(*ic);
// find non-zeros U[j][i] below U[k][i] for j in (k, i]
// 1. Go to ith row in L (== ith column in U)
cur_type irow= begin<row>(L); irow+= i;
// 2. Find nonzeros with col() in (k, i]
icur_type jc= begin<nz>(irow), jend= end<nz>(irow);
while (col_l(*jc) <= k) ++jc;
while (col_l(*--jend) > i);
++jend;
for (; jc != jend; ++jc) {
size_type j= col_l(*jc);
U.lvalue(j, i)-= d * U[k][j];
}
// std::cout << "U after eliminating U[" << i << "][" << k << "] =\n" << U;
}
}
}
typename Collection<Matrix>::value_type
inline trace(const Matrix& matrix)
{
using math::zero;
typedef typename Collection<Matrix>::value_type value_type;
MTL_THROW_IF(num_rows(matrix) != num_cols(matrix), matrix_not_square());
// If matrix is empty then the result is the identity from the default-constructed value
if (num_rows(matrix) == 0) {
value_type ref;
return zero(ref);
}
value_type value= matrix[0][0];
for (unsigned i= 1; i < num_rows(matrix); i++)
value+= matrix[i][i];
return value;
}
void factorize(const Matrix& A, L_type& L, U_type& U, boost::mpl::true_)
{
using namespace mtl; using namespace mtl::tag; using mtl::traits::range_generator;
using math::reciprocal;
MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
mtl::vampir_trace<5038> tracer;
typedef typename mtl::Collection<Matrix>::value_type value_type;
typedef typename mtl::Collection<Matrix>::size_type size_type;
typedef mtl::mat::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
typedef mtl::mat::compressed2D<value_type, para> LU_type;
LU_type LU(A);
typedef typename range_generator<row, LU_type>::type cur_type;
typedef typename range_generator<nz, cur_type>::type icur_type;
typename mtl::traits::col<LU_type>::type col(LU);
typename mtl::traits::value<LU_type>::type value(LU);
mtl::vec::dense_vector<value_type, mtl::vec::parameters<> > inv_dia(num_rows(A));
cur_type ic= begin<row>(LU), iend= end<row>(LU);
for (size_type i= 0; ic != iend; ++ic, ++i) {
for (icur_type kc= begin<nz>(ic), kend= end<nz>(ic); kc != kend; ++kc) {
size_type k= col(*kc);
if (k == i) break;
value_type aik= value(*kc) * inv_dia[k];
value(*kc, aik);
for (icur_type jc= kc + 1; jc != kend; ++jc)
value(*jc, value(*jc) - aik * LU[k][col(*jc)]);
// std::cout << "LU after eliminating A[" << i << "][" << k << "] =\n" << LU;
}
inv_dia[i]= reciprocal(LU[i][i]);
}
invert_diagonal(LU);
L= strict_lower(LU);
U= upper(LU);
}
Scalar finiteDifferenceIncrement() const
{
using math::sqrt;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
int main( int argc, const char * argv[] )
{
args_t args = args_t( argc, argv );
coverage_t::const_iterator cit;
coverage_t coverage;
vector< pair< int, int > > data;
bam1_t * const in_bam = bam_init1();
while ( args.bamin->next( in_bam ) ) {
aligned_t read( in_bam );
coverage.include( read );
}
for ( cit = coverage.begin(); cit != coverage.end(); ++cit ) {
map< elem_t, int >::const_iterator it;
if ( cit->op != MATCH )
continue;
it = cit->obs.begin();
if ( it == cit->obs.end() )
continue;
int cov = it->second;
int max = it->second;
for ( ++it; it != cit->obs.end(); ++it ) {
cov += it->second;
if ( it->second > max )
max = it->second;
}
for ( it = cit->obs.begin(); it != cit->obs.end(); ++it )
if ( it->second && it->second != max )
data.push_back( make_pair( cov, cov - it->second ) );
}
rateclass_t rc( data, 3 );
double lg_L, aicc;
vector< pair< double, double > > params;
rc( lg_L, aicc, params );
const double bg = weighted_harmonic_mean( params );
const double lg_bg = log( bg );
const double lg_invbg = log( 1.0 - bg );
params_json_dump( stderr, lg_L, aicc, params, bg );
for ( cit = coverage.begin(); cit != coverage.end(); ++cit ) {
map< elem_t, int >::const_iterator it;
if ( cit->op != MATCH )
continue;
it = cit->obs.begin();
if ( it == cit->obs.end() )
continue;
int cov = it->second;
int max = it->second;
for ( ++it; it != cit->obs.end(); ++it ) {
cov += it->second;
if ( it->second > max )
max = it->second;
}
string css;
for ( it = cit->obs.begin(); it != cit->obs.end(); ++it )
if ( it->second == max ) {
string elem;
it->first.get_seq( elem );
css.append( elem );
css.push_back( '/' );
}
// erase the trailing slash, in a compatible way
css.erase( --css.end() );
for ( it = cit->obs.begin(); it != cit->obs.end(); ++it ) {
string elem;
if ( !it->second || it->second == max )
continue;
const double prob = prob_background( lg_bg, lg_invbg, cov, it->second );
if ( prob >= args.cutoff )
continue;
fprintf( stdout, "%d\t%s\t%d\t", cit->col + 1, css.c_str(), cov );
it->first.get_seq( elem );
fprintf( stdout, "%s", elem.c_str() );
fprintf( stdout, ":%d:%.3e\n", it->second, prob );
}
fflush( stdout );
}
bam_destroy1( in_bam );
return 0;
}
static inline void init(Value& value)
{
using math::zero;
value= zero(value);
}
static inline void init(Value& value)
{
using math::identity;
value= identity(math::min<Value>(), value);
}
static inline void init(Value& value)
{
using math::one;
value= one(value);
}
/// Value of matrix entry
value_type operator()(size_type r, size_type c) const
{
using math::zero;
utilities::maybe<size_type> o= offset(r, c);
return o ? data[o.value()] : zero(value_type());
}
void
RTL::set_rtl_item()
{
_navigator->set_can_loiter_at_sp(false);
const home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
// check if we are pretty close to home already
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
// if we are close to home we do not climb as high, otherwise we climb to return alt
float climb_alt = home.alt + _param_return_alt.get();
// we are close to home, limit climb to min
if (home_dist < _param_rtl_min_dist.get()) {
climb_alt = home.alt + _param_descend_alt.get();
}
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = gpos.lat;
_mission_item.lon = gpos.lon;
_mission_item.altitude = climb_alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
// don't change altitude
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = gpos.alt;
_mission_item.altitude_is_relative = false;
// use home yaw if close to home
/* check if we are pretty close to home already */
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = home.yaw;
} else {
// use current heading to home
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt));
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
break;
}
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
_mission_item.altitude_is_relative = false;
// except for vtol which might be still off here and should point towards this location
const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
} else {
_mission_item.yaw = home.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
/* disable previous setpoint to prevent drift */
pos_sp_triplet->previous.valid = false;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt));
break;
}
case RTL_STATE_LOITER: {
const bool autoland = (_param_land_delay.get() > FLT_EPSILON);
// don't change altitude
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = gpos.alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = home.yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = max(_param_land_delay.get(), 0.0f);
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)get_time_inside(_mission_item));
} else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering");
}
break;
}
case RTL_STATE_LAND: {
// land at home position
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.yaw = home.yaw;
_mission_item.altitude = home.alt;
_mission_item.altitude_is_relative = false;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
set_idle_item(&_mission_item);
set_return_alt_min(false);
break;
}
default:
break;
}
reset_mission_item_reached();
/* execute command if set. This is required for commands like VTOL transition */
if (!item_contains_position(_mission_item)) {
issue_command(_mission_item);
}
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(_mission_item);
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}
static void setVariableFromIRQ(const unsigned index, const Value value)
{
auto& self = getInstance();
self.vars_.at(index) = Scalar(value);
self.update_flags_.at(index) = true;
}
void set(const Value x)
{
static_assert(Index < NumVariables, "Debug variable index out of range");
vars_[Index] = Scalar(x);
}
CRay2 CRay2::operator+ (const math::CVector2& Other) const
{
return CRay2(this->Start + Other, this->End + Other);
}
