void ins_init() {
#if USE_INS_NAV_INIT
ins_ltp_initialised = TRUE;
/** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
ins_ltp_def.hmsl = NAV_ALT0;
#else
ins_ltp_initialised = FALSE;
#endif
ins_baro_initialised = FALSE;
init_median_filter(&baro_median);
ins_update_on_agl = FALSE;
init_median_filter(&sonar_median);
ins_sonar_offset = INS_SONAR_OFFSET;
vff_init(0., 0., 0., 0.);
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_ltp_pos);
INT32_VECT3_ZERO(ins_ltp_speed);
INT32_VECT3_ZERO(ins_ltp_accel);
}
/**
* Initialization function
*/
void lidar_lite_init(void)
{
lidar.trans.status = I2CTransDone;
lidar.addr = LIDAR_LITE_I2C_ADDR;
lidar.status = LIDAR_INIT_RANGING;
lidar.update_agl = USE_LIDAR_LITE_AGL;
init_median_filter(&lidar_filter);
}
/**
* Initialize the opticflow calculator
* @param[out] *opticflow The new optical flow calculator
* @param[in] *w The image width
* @param[in] *h The image height
*/
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
{
init_median_filter(&vel_x_filt);
init_median_filter(&vel_y_filt);
/* Create the image buffers */
image_create(&opticflow->img_gray, w, h, IMAGE_GRAYSCALE);
image_create(&opticflow->prev_img_gray, w, h, IMAGE_GRAYSCALE);
/* Set the previous values */
opticflow->got_first_img = false;
FLOAT_RATES_ZERO(opticflow->prev_rates);
/* Set the default values */
opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE;
opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON
opticflow->derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X;
opticflow->derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y;
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL;
opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER;
opticflow->kalman_filter = OPTICFLOW_KALMAN_FILTER;
opticflow->kalman_filter_process_noise = OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE;
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
opticflow->fast9_padding = OPTICFLOW_FAST9_PADDING;
opticflow->fast9_rsize = 512;
opticflow->fast9_ret_corners = malloc(sizeof(struct point_t) * opticflow->fast9_rsize);
}
/**
* Initialization function
*/
void lidar_sf11_init(void)
{
lidar_sf11.trans.status = I2CTransDone;
lidar_sf11.addr = LIDAR_SF11_I2C_ADDR;
lidar_sf11.status = LIDAR_SF11_REQ_READ;
lidar_sf11.update_agl = USE_LIDAR_SF11_AGL;
lidar_sf11.compensate_rotation = LIDAR_SF11_COMPENSATE_ROTATION;
lidar_sf11.distance = 0;
lidar_sf11.distance_raw = 0;
init_median_filter(&lidar_sf11_filter);
}
void ins_init(void) {
#if USE_INS_NAV_INIT
ins_init_origin_from_flightplan();
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#endif
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
ins_impl.baro_initialized = FALSE;
#if USE_SONAR
ins_impl.update_on_agl = FALSE;
init_median_filter(&ins_impl.sonar_median);
ins_impl.sonar_offset = INS_SONAR_OFFSET;
#endif
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
/* init vertical and horizontal filters */
#if USE_VFF_EXTENDED
vff_init(0., 0., 0., 0.);
#else
vff_init(0., 0., 0.);
#endif
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
