/**
* Initialize the optical flow landing module
*/
void vertical_ctrl_module_init(void)
{
unsigned int i;
of_landing_ctrl.agl = 0.0f;
of_landing_ctrl.agl_lp = 0.0f;
of_landing_ctrl.vel = 0.0f;
of_landing_ctrl.divergence_setpoint = 0.0f;
of_landing_ctrl.cov_set_point = -0.025f;
of_landing_ctrl.cov_limit = 0.0010f; //1.0f; // for cov(uz,div)
of_landing_ctrl.lp_factor = 0.95f;
of_landing_ctrl.pgain = OPTICAL_FLOW_LANDING_PGAIN;
of_landing_ctrl.igain = OPTICAL_FLOW_LANDING_IGAIN;
of_landing_ctrl.dgain = OPTICAL_FLOW_LANDING_DGAIN;
of_landing_ctrl.sum_err = 0.0f;
of_landing_ctrl.nominal_thrust = 0.710f; //0.666f; // 0.640 with small battery
of_landing_ctrl.VISION_METHOD = OPTICAL_FLOW_LANDING_VISION_METHOD;
of_landing_ctrl.CONTROL_METHOD = OPTICAL_FLOW_LANDING_CONTROL_METHOD;
of_landing_ctrl.COV_METHOD = OPTICAL_FLOW_LANDING_COV_METHOD;
of_landing_ctrl.delay_steps = 40;
of_landing_ctrl.pgain_adaptive = 10.0;
of_landing_ctrl.igain_adaptive = 0.25;
of_landing_ctrl.dgain_adaptive = 0.00;
struct timespec spec;
clock_gettime(CLOCK_REALTIME, &spec);
previous_time = spec.tv_nsec / 1.0E6;
//previous_time = time(NULL);
// clear histories:
ind_hist = 0;
for (i = 0; i < COV_WINDOW_SIZE; i++) {
thrust_history[i] = 0;
divergence_history[i] = 0;
}
// reset errors, thrust, divergence, etc.:
previous_err = 0.0f;
previous_cov_err = 0.0f;
normalized_thrust = 0.0f;
divergence = 0.0f;
divergence_vision = 0.0f;
divergence_vision_dt = 0.0f;
cov_div = 0.0f;
dt = 0.0f;
pstate = of_landing_ctrl.pgain;
pused = pstate;
vision_message_nr = 1;
previous_message_nr = 0;
of_landing_ctrl.agl_lp = 0.0f;
landing = 0;
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(OPTICAL_FLOW_LANDING_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
// Subscribe to the optical flow estimator:
AbiBindMsgOPTICAL_FLOW(OPTICAL_FLOW_LANDING_OPTICAL_FLOW_ID, &optical_flow_ev, vertical_ctrl_optical_flow_cb);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DIVERGENCE, send_divergence);
}
void vertical_ctrl_module_init(void)
{
v_ctrl.agl = 0.0f;
v_ctrl.setpoint = 1.0f;
v_ctrl.pgain = VERTICAL_CTRL_MODULE_PGAIN;
v_ctrl.igain = VERTICAL_CTRL_MODULE_IGAIN;
v_ctrl.sum_err = 0.0f;
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(VERTICAL_CTRL_MODULE_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
}
void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_int.ltp_def);
ins_int.ltp_initialized = true;
#else
ins_int.ltp_initialized = false;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = false;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = false;
ins_int.hf_realign = false;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
}
/**
* Initialize the optical flow module for the bottom camera
*/
void opticflow_module_init(void)
{
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb);
// Set the opticflow state to 0
FLOAT_RATES_ZERO(opticflow_state.rates);
opticflow_state.agl = 0;
// Initialize the opticflow calculation
opticflow_got_result = false;
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
#endif
}
void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_from_flightplan();
ins_int.ltp_initialized = TRUE;
#else
ins_int.ltp_initialized = FALSE;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = FALSE;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = FALSE;
ins_int.hf_realign = FALSE;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
}
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 = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_impl.vf_reset = FALSE;
ins_impl.hf_realign = FALSE;
/* init vertical and horizontal filters */
vff_init_zero();
#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 PERIODIC_TELEMETRY
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
}