Method init

libraries/AP_SmartRTL/AP_SmartRTL.cpp:94–135

initialise safe rtl including setting up background processes

// initialise safe rtl including setting up background processes
void AP_SmartRTL::init()
{
    // protect against repeated call to init
    if (_path != nullptr) {
        return;
    }

    // constrain the path length, in case the user decided to make the path unreasonably long.
    _points_max.set(constrain_int16(_points_max, 0, SMARTRTL_POINTS_MAX));

    // check if user has disabled SmartRTL
    if (_points_max == 0 || !is_positive(_accuracy)) {
        return;
    }

    // allocate arrays
    _path = (Vector3f*)calloc(_points_max, sizeof(Vector3f));

    _prune.loops_max = _points_max * SMARTRTL_PRUNING_LOOP_BUFFER_LEN_MULT;
    _prune.loops = (prune_loop_t*)calloc(_prune.loops_max, sizeof(prune_loop_t));

    _simplify.stack_max = _points_max * SMARTRTL_SIMPLIFY_STACK_LEN_MULT;
    _simplify.stack = (simplify_start_finish_t*)calloc(_simplify.stack_max, sizeof(simplify_start_finish_t));

    // check if memory allocation failed
    if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) {
        log_action(Action::DEACTIVATED_INIT_FAILED);
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed");
        free(_path);
        free(_prune.loops);
        free(_simplify.stack);
        return;
    }

    _path_points_max = _points_max;

    // when running the example sketch, we want the cleanup tasks to run when we tell them to, not in the background (so that they can be timed.)
    if (!_example_mode){
        // register background cleanup to run in IO thread
        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SmartRTL::run_background_cleanup, void));
    }
}

// returns number of points on the path
uint16_t AP_SmartRTL::get_num_points() const