bool AP_Arming::arm_checks(ArmingMethod method)
{
// ensure the GPS drivers are ready on any final changes
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
if (!AP::gps().prepare_for_arming()) {
return false;
}
}
// check system health on arm as well as prearm
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_SYSTEM)) {
if (!system_checks(true)) {
return false;
}
}
// note that this will prepare DataFlash to start logging
// so should be the last check to be done before arming
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_LOGGING)) {
DataFlash_Class *df = DataFlash_Class::instance();
df->PrepForArming();
if (!df->logging_started()) {
check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
return false;
}
}
return true;
}
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();
}
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_list_t packet;
mavlink_msg_log_request_list_decode(msg, &packet);
_log_listing = false;
_log_sending = false;
_log_num_logs = dataflash.get_num_logs();
if (_log_num_logs == 0) {
_log_next_list_entry = 0;
_log_last_list_entry = 0;
} else {
uint16_t last_log_num = dataflash.find_last_log();
_log_next_list_entry = packet.start;
_log_last_list_entry = packet.end;
if (_log_last_list_entry > last_log_num) {
_log_last_list_entry = last_log_num;
}
if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
_log_next_list_entry = last_log_num + 1 - _log_num_logs;
}
}
_log_listing = true;
handle_log_send_listing(dataflash);
}
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_list_t packet;
mavlink_msg_log_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = false;
_log_sending = false;
_log_num_logs = dataflash.get_num_logs();
if (_log_num_logs == 0) {
return;
}
int16_t last_log_num = dataflash.find_last_log();
_log_next_list_entry = packet.start;
_log_last_list_entry = packet.end;
if (_log_last_list_entry > last_log_num) {
_log_last_list_entry = last_log_num;
}
if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
_log_next_list_entry = last_log_num + 1 - _log_num_logs;
}
_log_listing = true;
handle_log_send_listing(dataflash);
}
void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
const Vector3f &gyro,
uint64_t sample_us)
{
float dt;
if (_imu._gyro_raw_sample_rates[instance] <= 0) {
return;
}
dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
// call gyro_sample hook if any
AP_Module::call_hook_gyro_sample(instance, dt, gyro);
// compute delta angle
Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
// compute coning correction
// see page 26 of:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// see also examples/coning.py
Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
_imu._last_delta_angle[instance] * (1.0f / 6.0f));
delta_coning = delta_coning % delta_angle;
delta_coning *= 0.5f;
// integrate delta angle accumulator
// the angles and coning corrections are accumulated separately in the
// referenced paper, but in simulation little difference was found between
// integrating together and integrating separately (see examples/coning.py)
_imu._delta_angle_acc[instance] += delta_angle + delta_coning;
_imu._delta_angle_acc_dt[instance] += dt;
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
_imu._gyro_filter[instance].reset();
}
_imu._new_gyro_data[instance] = true;
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
uint64_t now = AP_HAL::micros64();
struct log_GYRO pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+instance)),
time_us : now,
sample_us : sample_us?sample_us:now,
GyrX : gyro.x,
GyrY : gyro.y,
GyrZ : gyro.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
void SoloGimbal::write_logs()
{
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash == nullptr) {
return;
}
uint32_t tstamp = AP_HAL::millis();
Vector3f eulerEst;
Quaternion quatEst;
_ekf.getQuat(quatEst);
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
struct log_Gimbal1 pkt1 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
time_ms : tstamp,
delta_time : _log_dt,
delta_angles_x : _log_del_ang.x,
delta_angles_y : _log_del_ang.y,
delta_angles_z : _log_del_ang.z,
delta_velocity_x : _log_del_vel.x,
delta_velocity_y : _log_del_vel.y,
delta_velocity_z : _log_del_vel.z,
joint_angles_x : _measurement.joint_angles.x,
joint_angles_y : _measurement.joint_angles.y,
joint_angles_z : _measurement.joint_angles.z
};
dataflash->WriteBlock(&pkt1, sizeof(pkt1));
struct log_Gimbal2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
time_ms : tstamp,
est_sta : (uint8_t) _ekf.getStatus(),
est_x : eulerEst.x,
est_y : eulerEst.y,
est_z : eulerEst.z,
rate_x : _ang_vel_dem_rads.x,
rate_y : _ang_vel_dem_rads.y,
rate_z : _ang_vel_dem_rads.z,
target_x: _att_target_euler_rad.x,
target_y: _att_target_euler_rad.y,
target_z: _att_target_euler_rad.z
};
dataflash->WriteBlock(&pkt2, sizeof(pkt2));
_log_dt = 0;
_log_del_ang.zero();
_log_del_vel.zero();
}
bool AP_GPS_Backend::should_df_log() const
{
DataFlash_Class *instance = DataFlash_Class::instance();
if (instance == nullptr) {
return false;
}
if (gps._log_gps_bit == (uint32_t)-1) {
return false;
}
if (!instance->should_log(gps._log_gps_bit)) {
return false;
}
return true;
}
/**
trigger sending of log messages if there are some pending
*/
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
uint16_t txspace = comm_get_txspace(chan);
if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
// no space
return;
}
if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return;
}
uint32_t size, time_utc;
if (_log_next_list_entry == 0) {
size = 0;
time_utc = 0;
} else {
dataflash.get_log_info(_log_next_list_entry, size, time_utc);
}
mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
if (_log_next_list_entry == _log_last_list_entry) {
_log_listing = false;
} else {
_log_next_list_entry++;
}
}
void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio)
{
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
// record if the GCS has been receiving radio messages from
// the aircraft
if (packet.remrssi != 0) {
last_radio_status_remrssi_ms = hal.scheduler->millis();
}
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space - slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
}
//log rssi, noise, etc if logging Performance monitoring data
if (log_radio) {
dataflash.Log_Write_Radio(packet);
}
}
/**
trigger sending of log messages if there are some pending
*/
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
if (!HAVE_PAYLOAD_SPACE(chan, LOG_ENTRY)) {
// no space
return;
}
if (AP_HAL::millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return;
}
uint32_t size, time_utc;
if (_log_next_list_entry == 0) {
size = 0;
time_utc = 0;
} else {
dataflash.get_log_info(_log_next_list_entry, size, time_utc);
}
mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
if (_log_next_list_entry == _log_last_list_entry) {
_log_listing = false;
} else {
_log_next_list_entry++;
}
}
/**
handle request to erase log data
*/
void GCS_MAVLINK::handle_log_request_erase(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_erase_t packet;
mavlink_msg_log_erase_decode(msg, &packet);
dataflash.EraseAll();
}
/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
state.rollRate, state.pitchRate, state.yawRate,
&p, &q, &r);
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// log the contents of the log_tuning structure to dataflash
void AP_TECS::log_data(DataFlash_Class &dataflash, uint8_t msgid)
{
log_tuning.head1 = HEAD_BYTE1;
log_tuning.head2 = HEAD_BYTE2;
log_tuning.msgid = msgid;
dataflash.WriteBlock(&log_tuning, sizeof(log_tuning));
}
/**
trigger sending of log data if there are some pending
*/
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space < MAVLINK_MSG_ID_LOG_DATA_LEN) {
// no space
return false;
}
if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return false;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
uint8_t data[90];
if (len > 90) {
len = 90;
}
ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
if (ret < 90) {
memset(&data[ret], 0, 90-ret);
}
mavlink_msg_log_data_send(chan, _log_num_data, _log_data_offset, ret, data);
_log_data_offset += len;
_log_data_remaining -= len;
if (ret < 90 || _log_data_remaining == 0) {
_log_sending = false;
}
return true;
}
// read - read the voltage and current for all instances
void
AP_BattMonitor::read()
{
for (uint8_t i=0; i<_num_instances; i++) {
if (drivers[i] != nullptr && _params[i].type() != AP_BattMonitor_Params::BattMonitor_TYPE_NONE) {
drivers[i]->read();
drivers[i]->update_resistance_estimate();
}
}
DataFlash_Class *df = DataFlash_Class::instance();
if (df->should_log(_log_battery_bit)) {
df->Log_Write_Current();
df->Log_Write_Power();
}
check_failsafes();
}
void DataFlashTest_AllTypes::Log_Write_TypeMessages()
{
log_num = dataflash.find_last_log();
hal.console->printf("Using log number %u\n", log_num);
struct log_TYP1 typ1 = {
LOG_PACKET_HEADER_INIT(LOG_TYP1_MSG),
time_us : AP_HAL::micros64(),
a : { -32768, 32767, 1, -1, 0, 17 }, // int16[32]
void DataFlashTest_AllTypes::Log_Write_TypeMessages()
{
dataflash.StartNewLog();
log_num = dataflash.find_last_log();
hal.console->printf("Using log number %u\n", log_num);
struct log_TYP1 typ1 = {
LOG_PACKET_HEADER_INIT(LOG_TYP1_MSG),
time_us : AP_HAL::micros64(),
b : -17, // int8_t
B : 42, // uint8_t
h : -12372, // int16_t
H : 19812, // uint16_t
i : -98234729, // int32_t
I : 74627293, // uint32_t
f : 35.87654, // float
d : 67.7393274658293, // double
n : { 'A', 'B', 'C', 'D' }, // char[4]
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();
}
void DataFlashTest_AllTypes::flush_dataflash(DataFlash_Class &_dataflash)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
_dataflash.flush();
#else
// flush is not available on e.g. px4 as it would be a somewhat
// dangerous operation, but if we wait long enough (at time of
// writing, 2 seconds, see DataFlash_File::_io_timer) the data
// will go out.
hal.scheduler->delay(3000);
#endif
}
/**
handle request for log data
*/
void GCS_MAVLINK::handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_data_t packet;
mavlink_msg_log_request_data_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = false;
if (!_log_sending || _log_num_data != packet.id) {
_log_sending = false;
uint16_t num_logs = dataflash.get_num_logs();
int16_t last_log_num = dataflash.find_last_log();
if (packet.id > last_log_num || packet.id < last_log_num + 1 - num_logs) {
return;
}
uint32_t time_utc, size;
dataflash.get_log_info(packet.id, size, time_utc);
_log_num_data = packet.id;
_log_data_size = size;
uint16_t end;
dataflash.get_log_boundaries(packet.id, _log_data_page, end);
}
_log_data_offset = packet.ofs;
if (_log_data_offset >= _log_data_size) {
_log_data_remaining = 0;
} else {
_log_data_remaining = _log_data_size - _log_data_offset;
}
if (_log_data_remaining > packet.count) {
_log_data_remaining = packet.count;
}
_log_sending = true;
handle_log_send(dataflash);
}
void OpticalFlow::Log_Write_Optflow()
{
DataFlash_Class *instance = DataFlash_Class::instance();
if (instance == nullptr) {
return;
}
if (_log_bit != (uint32_t)-1 &&
!instance->should_log(_log_bit)) {
return;
}
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_us : AP_HAL::micros64(),
surface_quality : _state.surface_quality,
flow_x : _state.flowRate.x,
flow_y : _state.flowRate.y,
body_x : _state.bodyRate.x,
body_y : _state.bodyRate.y
};
instance->WriteBlock(&pkt, sizeof(pkt));
}
bool AP_Arming::arm_checks(uint8_t method)
{
// ensure the GPS drivers are ready on any final changes
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
if (!AP::gps().prepare_for_arming()) {
return false;
}
}
// note that this will prepare DataFlash to start logging
// so should be the last check to be done before arming
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_LOGGING)) {
DataFlash_Class *df = DataFlash_Class::instance();
df->PrepForArming();
if (!df->logging_started()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Arm: Logging not started");
return false;
}
}
return true;
}
bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_TYPE _type)
{
if (_sensor_mask == 0) {
return false;
}
if (!initialised) {
return false;
}
if (_instance != instance) {
return false;
}
if (_type != type) {
return false;
}
if (data_write_offset >= _required_count) {
return false;
}
DataFlash_Class *dataflash = DataFlash_Class::instance();
#define MASK_LOG_ANY 0xFFFF
if (!dataflash->should_log(MASK_LOG_ANY)) {
return false;
}
return true;
}
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_message(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
handle_log_request_list(msg, dataflash);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
handle_log_request_data(msg, dataflash);
break;
case MAVLINK_MSG_ID_LOG_ERASE:
dataflash.EraseAll();
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
_log_sending = false;
break;
}
}
/**
trigger sending of log data if there are some pending
*/
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
if (!HAVE_PAYLOAD_SPACE(chan, LOG_DATA)) {
// no space
return false;
}
if (AP_HAL::millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return false;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
mavlink_log_data_t packet;
if (len > 90) {
len = 90;
}
ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
if (ret < 90) {
memset(&packet.data[ret], 0, 90-ret);
}
packet.ofs = _log_data_offset;
packet.id = _log_num_data;
packet.count = ret;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet,
MAVLINK_MSG_ID_LOG_DATA_MIN_LEN,
MAVLINK_MSG_ID_LOG_DATA_LEN,
MAVLINK_MSG_ID_LOG_DATA_CRC);
_log_data_offset += len;
_log_data_remaining -= len;
if (ret < 90 || _log_data_remaining == 0) {
_log_sending = false;
}
return true;
}
/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
float yaw;
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_us : hal.scheduler->micros64(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void AP_InertialSensor::BatchSampler::push_data_to_log()
{
if (!initialised) {
return;
}
if (_sensor_mask == 0) {
return;
}
if (data_write_offset - data_read_offset < samples_per_msg) {
// insuffucient data to pack a packet
return;
}
const uint32_t now = AP_HAL::millis();
if (now - last_sent_ms < push_interval_ms) {
// avoid flooding DataFlash's buffer
return;
}
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash == nullptr) {
// should not have been called
return;
}
// possibly send isb header:
if (!isbh_sent && data_read_offset == 0) {
float sample_rate = 0; // avoid warning about uninitialised values
switch(type) {
case IMU_SENSOR_TYPE_GYRO:
sample_rate = _imu._gyro_raw_sample_rates[instance];
break;
case IMU_SENSOR_TYPE_ACCEL:
sample_rate = _imu._accel_raw_sample_rates[instance];
break;
}
if (!dataflash->Log_Write_ISBH(isb_seqnum,
type,
instance,
multiplier,
_required_count,
measurement_started_us,
sample_rate)) {
// buffer full?
return;
}
isbh_sent = true;
}
// pack and send a data packet:
if (!dataflash->Log_Write_ISBD(isb_seqnum,
data_read_offset/samples_per_msg,
&data_x[data_read_offset],
&data_y[data_read_offset],
&data_z[data_read_offset])) {
// maybe later?!
return;
}
data_read_offset += samples_per_msg;
last_sent_ms = AP_HAL::millis();
if (data_read_offset >= _required_count) {
// that was the last one. Clean up:
data_read_offset = 0;
isb_seqnum++;
isbh_sent = false;
// rotate to next instance:
rotate_to_next_sensor();
data_write_offset = 0; // unlocks writing process
}
}
// Initialise the filter
bool NavEKF2::InitialiseFilter(void)
{
if (_enable == 0) {
return false;
}
imuSampleTime_us = AP_HAL::micros64();
// see if we will be doing logging
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash != nullptr) {
logging.enabled = dataflash->log_replay();
}
if (core == nullptr) {
// don't run multiple filters for 1 IMU
const AP_InertialSensor &ins = _ahrs->get_ins();
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask);
// count IMUs from mask
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
num_cores++;
}
}
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
_enable.set(0);
return false;
}
core = new NavEKF2_core[num_cores];
if (core == nullptr) {
_enable.set(0);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
return false;
}
// set the IMU index for the cores
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
if(!core[num_cores].setup_core(this, i, num_cores)) {
return false;
}
num_cores++;
}
}
// Set the primary initially to be the lowest index
primary = 0;
}
// initialse the cores. We return success only if all cores
// initialise successfully
bool ret = true;
for (uint8_t i=0; i<num_cores; i++) {
ret &= core[i].InitialiseFilterBootstrap();
}
check_log_write();
return ret;
}