/*
send OPTICAL_FLOW message
*/
void GCS_MAVLINK::send_opticalflow(AP_AHRS_NavEKF &ahrs, const OpticalFlow &optflow)
{
// exit immediately if no optical flow sensor or not healthy
if (!optflow.healthy()) {
return;
}
// get rates from sensor
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
float hagl = 0;
if (ahrs.have_inertial_nav()) {
ahrs.get_NavEKF().getHAGL(hagl);
}
// populate and send message
mavlink_msg_optical_flow_send(
chan,
hal.scheduler->millis(),
0, // sensor id is zero
flowRate.x,
flowRate.y,
bodyRate.x,
bodyRate.y,
optflow.quality(),
hagl); // ground distance (in meters) set to zero
}
void ReplayVehicle::setup(void)
{
load_parameters();
// we pass zero log structures, as we will be outputting the log
// structures we need manually, to prevent FMT duplicates
dataflash.Init(log_structure, 0);
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
EKF2.set_enable(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
// constructor
SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
_ahrs(ahrs),
_main_ekf(ahrs.get_NavEKF_const()),
state(*reinterpret_cast<struct state_elements *>(&states)),
FiltInit(false),
lastMagUpdate(0)
{
AP_Param::setup_object_defaults(this, var_info);
}
void ReplayVehicle::setup(void) {
dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
/*
call any AHRS_update hooks
*/
void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
{
#if AP_MODULE_SUPPORTED
if (hooks[HOOK_AHRS_UPDATE] == nullptr) {
// avoid filling in AHRS_state
return;
}
/*
construct AHRS_state structure
*/
struct AHRS_state state {};
state.structure_version = AHRS_state_version;
state.time_us = AP_HAL::micros64();
if (!ahrs.initialised()) {
state.status = AHRS_STATUS_INITIALISING;
} else if (ahrs.healthy()) {
state.status = AHRS_STATUS_HEALTHY;
} else {
state.status = AHRS_STATUS_UNHEALTHY;
}
Quaternion q;
q.from_rotation_matrix(ahrs.get_rotation_body_to_ned());
state.quat[0] = q[0];
state.quat[1] = q[1];
state.quat[2] = q[2];
state.quat[3] = q[3];
state.eulers[0] = ahrs.roll;
state.eulers[1] = ahrs.pitch;
state.eulers[2] = ahrs.yaw;
Location loc;
if (ahrs.get_origin(loc)) {
state.origin.initialised = true;
state.origin.latitude = loc.lat;
state.origin.longitude = loc.lng;
state.origin.altitude = loc.alt*0.01f;
}
if (ahrs.get_position(loc)) {
state.position.available = true;
state.position.latitude = loc.lat;
state.position.longitude = loc.lng;
state.position.altitude = loc.alt*0.01f;
}
Vector3f pos;
if (ahrs.get_relative_position_NED(pos)) {
state.relative_position[0] = pos[0];
state.relative_position[1] = pos[1];
state.relative_position[2] = pos[2];
}
const Vector3f &gyro = ahrs.get_gyro();
state.gyro_rate[0] = gyro[0];
state.gyro_rate[1] = gyro[1];
state.gyro_rate[2] = gyro[2];
const Vector3f &accel_ef = ahrs.get_accel_ef();
state.accel_ef[0] = accel_ef[0];
state.accel_ef[1] = accel_ef[0];
state.accel_ef[2] = accel_ef[0];
for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) {
ap_hook_AHRS_update_fn_t fn = reinterpret_cast<ap_hook_AHRS_update_fn_t>(h->symbol);
fn(&state);
}
#endif
}
