Scheduler::Scheduler(const Scheduler::conf_t config) :
schedule_strategy_(config.schedule_strategy),
debug_(config.debug),
task_count_(0),
current_schedule_slot_(0)
{
// allocate memory for tasks
for(max_task_count_ = config.max_task_count; max_task_count_ > 0; max_task_count_--)
{
tasks_ = (Scheduler_task*)malloc(sizeof(Scheduler_task)*max_task_count_);
if(tasks_ != NULL)
{
break;
}
}
if(max_task_count_< config.max_task_count)
{
print_util_dbg_print("[Scheduler] constructor: tried to allocate task list for ");
print_util_dbg_print_num(config.max_task_count,10);
print_util_dbg_print(" tasks; only space for ");
print_util_dbg_print_num(max_task_count_,10);
print_util_dbg_print("\r\n");
}
}
static mav_result_t state_telemetry_toggle_remote_use(state_t* state, mavlink_command_long_t* packet)
{
mav_result_t result = MAV_RESULT_UNSUPPORTED;
if ( packet->param1 == 1)
{
state->remote_active = 1;
state->use_mode_from_remote = 1;
print_util_dbg_print("Remote control activated\r\n");
result = MAV_RESULT_ACCEPTED;
}
else if (packet->param1 == 0)
{
state->remote_active = 0;
state->use_mode_from_remote = 0;
print_util_dbg_print("Remote control disactivated\r\n");
result = MAV_RESULT_ACCEPTED;
}
return result;
}
void onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg)
{
// Onboard parameters storage
if (msg->param1 == 0)
{
// read parameters from flash
print_util_dbg_print("Reading from flashc...\r\n");
if(onboard_parameters_read_parameters_from_flashc(onboard_parameters))
{
// TODO: update simulation calibration values
//simulation_calib_set(&sim_model);
}
}
else if (msg->param1 == 1)
{
// write parameters to flash
//print_util_dbg_print("No Writing to flashc\n");
print_util_dbg_print("Writing to flashc\r\n");
onboard_parameters_write_parameters_to_flashc(onboard_parameters);
}
mavlink_message_t ack_msg;
mavlink_msg_command_ack_pack( onboard_parameters->mavlink_stream->sysid,
onboard_parameters->mavlink_stream->compid,
&ack_msg,
MAV_CMD_PREFLIGHT_STORAGE,
MAV_RESULT_ACCEPTED );
mavlink_stream_send(onboard_parameters->mavlink_stream, &ack_msg);
}
mav_result_t onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg)
{
mav_result_t result = MAV_RESULT_DENIED;
// Onboard parameters storage
if (msg->param1 == 0)
{
// read parameters from flash
print_util_dbg_print("Reading from flashc...\r\n");
if(onboard_parameters_read_parameters_from_flashc(onboard_parameters))
{
result = MAV_RESULT_ACCEPTED;
// TODO: update simulation calibration values
//simulation_calib_set(&sim_model);
}
else
{
result = MAV_RESULT_DENIED;
}
}
else if (msg->param1 == 1)
{
// write parameters to flash
//print_util_dbg_print("No Writing to flashc\n");
print_util_dbg_print("Writing to flashc\r\n");
onboard_parameters_write_parameters_to_flashc(onboard_parameters);
result = MAV_RESULT_ACCEPTED;
}
return result;
}
DSTATUS disk_status (BYTE pdrv)
{
DSTATUS stat = STA_NOINIT;
int result = RES_ERROR;
if ((actual_status & STA_NOINIT)||(actual_status & STA_NODISK))
{
return STA_NOINIT;
}
// Only MMC supported
if (pdrv!=0)
{
return RES_PARERR;
}
uint8_t drive_num = MMC;
switch (drive_num)
{
case ATA :
//result = ATA_disk_status();
print_util_dbg_print("NO SUPPORTED! ATA status!\r\n");
// translate the result code here
result = RES_ERROR;
stat = STA_NODISK;
break;
case MMC :
//result = MMC_disk_status();
// translate the result code here
result = sd_spi_status();
if (result)
{
stat = 0;
}
else
{
stat = STA_NODISK;
}
break;
case USB :
//result = USB_disk_status();
print_util_dbg_print("NO SUPPORTED! USB status!\r\n");
// translate the result code here
result = RES_ERROR;
stat = STA_NODISK;
break;
}
return stat;
}
void onboard_parameters_add_parameter_float(onboard_parameters_t* onboard_parameters, float* val, const char* param_name)
{
onboard_parameters_set_t* param_set = onboard_parameters->param_set;
if( val == NULL )
{
print_util_dbg_print("[ONBOARD PARAMETER] Error: Null pointer!");
}
else
{
if( param_set->param_count < param_set->max_param_count )
{
onboard_parameters_entry_t* new_param = ¶m_set->parameters[param_set->param_count];
new_param->param = val;
strcpy( new_param->param_name, param_name );
new_param->data_type = MAV_PARAM_TYPE_REAL32;
new_param->param_name_length = strlen(param_name);
new_param->schedule_for_transmission = true;
param_set->param_count += 1;
}
else
{
print_util_dbg_print("[ONBOARD PARAMETER] Error: Cannot add more param\r\n");
}
}
}
void data_logging_add_parameter_float(data_logging_t* data_logging, float* val, const char* param_name)
{
data_logging_set_t* data_logging_set = data_logging->data_logging_set;
if( val == NULL )
{
print_util_dbg_print("[DATA LOGGING] Error: Null pointer!");
}
else
{
if( data_logging_set->data_logging_count < data_logging_set->max_data_logging_count )
{
data_logging_entry_t* new_param = &data_logging_set->data_log[data_logging_set->data_logging_count];
new_param->param = (double*) val;
strcpy( new_param->param_name, param_name );
new_param->data_type = MAV_PARAM_TYPE_REAL32;
data_logging_set->data_logging_count += 1;
}
else
{
print_util_dbg_print("[DATA LOGGING] Error: Cannot add more logging param.\r\n");
}
}
}
void scheduler_init(scheduler_t* scheduler, const scheduler_conf_t* config, const mavlink_stream_t* mavlink_stream)
{
// Init dependency
scheduler->mavlink_stream = mavlink_stream;
// Init schedule strategy
scheduler->schedule_strategy = config->schedule_strategy;
// Init debug mode
scheduler->debug = config->debug;
// Allocate memory for the task set
scheduler->task_set = malloc( sizeof(task_set_t) + sizeof(task_entry_t[config->max_task_count]) );
if ( scheduler->task_set != NULL )
{
scheduler->task_set->max_task_count = config->max_task_count;
}
else
{
print_util_dbg_print("[SCHEDULER] ERROR ! Bad memory allocation\r\n");
scheduler->task_set->max_task_count = 0;
}
scheduler->task_set->task_count = 0;
scheduler->task_set->current_schedule_slot = 0;
print_util_dbg_print("[SCHEDULER] Init\r\n");
}
static mode_flag_armed_t get_armed_flag(remote_t* remote)
{
const remote_mode_t* remote_mode = &remote->mode;
mode_flag_armed_t armed = remote_mode->current_desired_mode.ARMED;
// Get armed flag
if( remote_get_throttle(remote) < -0.95f &&
remote_get_yaw(remote) > 0.9f &&
remote_get_pitch(remote) > 0.9f &&
remote_get_roll(remote) > 0.9f )
{
// Both sticks in bottom right corners => arm
print_util_dbg_print("Arming!\r\n");
armed = ARMED_ON;
}
else if ( remote_get_throttle(remote) < -0.95f &&
remote_get_yaw(remote) < -0.9f &&
remote_get_pitch(remote) > 0.9f &&
remote_get_roll(remote) < -0.9f )
{
// Both sticks in bottom left corners => disarm
print_util_dbg_print("Disarming!\r\n");
armed = ARMED_OFF;
}
else
{
// Keep current flag
}
return armed;
}
DSTATUS disk_initialize (BYTE pdrv)
{
DSTATUS stat = STA_NOINIT;
int result;
actual_status = STA_NOINIT;
uint8_t drive_num;
if (pdrv==0)
{
drive_num = MMC;
}
else
{
return STA_NOINIT;
}
switch (drive_num)
{
case ATA :
//result = ATA_disk_initialize();
print_util_dbg_print("NO SUPPORTED! ATA init!\r\n");
// translate the result code here
result = RES_ERROR;
stat = STA_NODISK;
break;
case MMC :
//result = MMC_disk_initialize();
result = sd_spi_init();
if (result)
{
stat = 0;
actual_status = 0;
}
else
{
stat = STA_NOINIT;
}
break;
case USB :
//result = USB_disk_initialize();
print_util_dbg_print("NO SUPPORTED! USB init!\r\n");
// translate the result code here
result = RES_ERROR;
stat = STA_NODISK;
break;
}
return stat;
}
void neighbors_selection_extrapolate_or_delete_position(neighbors_t *neighbors)
{
int32_t i, ind, ind_sup;
uint32_t delta_t;
uint32_t actual_time = time_keeper_get_millis();
for (ind = 0; ind < neighbors->number_of_neighbors; ind++)
{
delta_t = actual_time- neighbors->neighbors_list[ind].time_msg_received;
if (delta_t >= NEIGHBOR_TIMEOUT_LIMIT_MS)
{
print_util_dbg_print("Suppressing neighbor number ");
print_util_dbg_print_num(ind,10);
print_util_dbg_print("\r\n");
// suppressing element ind
for (ind_sup = ind; ind_sup < (neighbors->number_of_neighbors - 1); ind_sup++)
{
neighbors->neighbors_list[ind_sup] = neighbors->neighbors_list[ind_sup + 1];
}
(neighbors->number_of_neighbors)--;
}
else if (delta_t > ORCA_TIME_STEP_MILLIS)
{
// extrapolating the last known position assuming a constant velocity
for(i = 0; i < 3; i++)
{
neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i] + neighbors->neighbors_list[ind].velocity[i] *((float)delta_t/1000);
}
//print_util_dbg_print("Extrapolated position (x100):");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10);
//print_util_dbg_print(", ");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10);
//print_util_dbg_print(", ");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10);
//print_util_dbg_print(")");
}
else
{
// taking the latest known position
for (i = 0; i < 3; i++)
{
neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i];
}
//print_util_dbg_print("Last known position (x100):");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10);
//print_util_dbg_print(", ");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10);
//print_util_dbg_print(", ");
//print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10);
//print_util_dbg_print(")");
}
}
}
bool scheduler_add_task(scheduler_t* scheduler, uint32_t repeat_period, task_run_mode_t run_mode, task_timing_mode_t timing_mode, task_priority_t priority, task_function_t call_function, task_argument_t function_argument, uint32_t task_id)
{
bool task_successfully_added = false;
task_set_t* ts = scheduler->task_set;
// Check if the scheduler is not full
if ( ts->task_count < ts->max_task_count )
{
// Check if there is already a task with this ID
bool id_is_unique = true;
for (uint32_t i = 0; i < ts->task_count; ++i)
{
if ( ts->tasks[i].task_id == task_id )
{
id_is_unique = false;
break;
}
}
// Add new task
if ( id_is_unique == true )
{
task_entry_t* new_task = &ts->tasks[ts->task_count];
new_task->call_function = call_function;
new_task->function_argument = function_argument;
new_task->task_id = task_id;
new_task->run_mode = run_mode;
new_task->timing_mode = timing_mode;
new_task->priority = priority;
new_task->repeat_period = repeat_period;
new_task->next_run = time_keeper_get_micros();
new_task->execution_time = 0;
new_task->delay_max = 0;
new_task->delay_avg = 0;
new_task->delay_var_squared = 0;
ts->task_count += 1;
task_successfully_added = true;
}
else
{
print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n");
task_successfully_added = false;
}
}
else
{
print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n");
task_successfully_added = false;
}
return task_successfully_added;
}
void print_util_dbg_log_value(const char* msg, int32_t value, char base)
{
print_util_dbg_print(msg);
if (base>1)
{
print_util_dbg_print_num(value, base);
}
print_util_dbg_print("\n");
}
void print_util_dbg_init_msg(const char* module_name, bool init_success)
{
print_util_dbg_print(module_name);
if (init_success == true)
{
print_util_dbg_print(" ok\r\n");
}
else
{
print_util_dbg_print(" INIT ERROR\r\n");
}
}
static mav_result_t position_estimation_set_new_home_position(position_estimation_t *pos_est, mavlink_command_long_t* packet)
{
mav_result_t result;
if(pos_est->state->mav_mode.ARMED == ARMED_OFF)
{
if (packet->param1 == 1)
{
// Set new home position to actual position
print_util_dbg_print("Set new home location to actual position.\r\n");
pos_est->local_position.origin = coord_conventions_local_to_global_position(pos_est->local_position);
print_util_dbg_print("New Home location: (");
print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
print_util_dbg_print(")\r\n");
}
else
{
// Set new home position from msg
print_util_dbg_print("[POSITION ESTIMATION] Set new home location. \r\n");
pos_est->local_position.origin.latitude = packet->param5;
pos_est->local_position.origin.longitude = packet->param6;
pos_est->local_position.origin.altitude = packet->param7;
print_util_dbg_print("New Home location: (");
print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10);
print_util_dbg_print(")\r\n");
}
pos_est->fence_set = false;
position_estimation_set_new_fence_origin(pos_est);
*pos_est->nav_plan_active = false;
result = MAV_RESULT_ACCEPTED;
}
else
{
result = MAV_RESULT_TEMPORARILY_REJECTED;
}
return result;
}
void mavlink_message_handler_msg_default_dbg(mavlink_message_t* msg)
{
if ((msg->sysid == MAVLINK_BASE_STATION_ID)&&(msg->msgid != MAVLINK_MSG_ID_MANUAL_CONTROL))
{
print_util_dbg_print("Received message with ID");
print_util_dbg_print_num(msg->msgid, 10);
print_util_dbg_print(" from system");
print_util_dbg_print_num(msg->sysid, 10);
print_util_dbg_print(" from component");
print_util_dbg_print_num(msg->compid,10);
print_util_dbg_print("\r\n");
}
}
bool Scheduler::add_task(uint32_t repeat_period,
Scheduler_task::task_function_t call_function,
Scheduler_task::task_argument_t function_argument,
Scheduler_task::priority_t priority,
Scheduler_task::timing_mode_t timing_mode,
Scheduler_task::run_mode_t run_mode,
int32_t task_id)
{
bool task_successfully_added = false;
// Check if the scheduler is not full
if (task_count_ < max_task_count_)
{
if (task_id == -1)
{
task_id = task_count_;
}
// Check if there is already a task with this ID
bool id_is_unique = true;
for (uint32_t i = 0; i < task_count_; ++i)
{
if (tasks_[i].get_id() == task_id)
{
id_is_unique = false;
break;
}
}
// Add new task
if (id_is_unique == true)
{
tasks_[task_count_++] = Scheduler_task(repeat_period, run_mode, timing_mode, priority, call_function, function_argument, task_id);
task_successfully_added = true;
}
else
{
print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n");
task_successfully_added = false;
}
}
else
{
print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n");
task_successfully_added = false;
}
return task_successfully_added;
}
void state_telemetry_set_mav_mode(state_t* state, uint32_t sysid, mavlink_message_t* msg)
{
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg,&packet);
// Check if this message is for this system and subsystem
// No component ID in mavlink_set_mode_t so no control
if ((uint8_t)packet.target_system == (uint8_t)sysid)
{
print_util_dbg_print("base_mode:");
print_util_dbg_print_num(packet.base_mode,10);
print_util_dbg_print(", custom mode:");
print_util_dbg_print_num(packet.custom_mode,10);
print_util_dbg_print("\r\n");
mav_mode_t new_mode;
new_mode.byte = packet.base_mode;
if (new_mode.ARMED == ARMED_ON)
{
if (state->mav_mode.ARMED == ARMED_OFF)
{
state_switch_to_active_mode(state, &state->mav_state);
}
}
else
{
state->mav_state = MAV_STATE_STANDBY;
}
state->mav_mode.ARMED = new_mode.ARMED;
state->mav_mode.MANUAL = new_mode.MANUAL;
//state->mav_mode.HIL = new_mode.HIL;
state->mav_mode.STABILISE = new_mode.STABILISE;
state->mav_mode.GUIDED = new_mode.GUIDED;
state->mav_mode.AUTO = new_mode.AUTO;
state->mav_mode.TEST = new_mode.TEST;
state->mav_mode.CUSTOM = new_mode.CUSTOM;
//state->mav_mode_custom = packet.custom_mode;
print_util_dbg_print("New mav mode:");
print_util_dbg_print_num(state->mav_mode.byte,10);
print_util_dbg_print("\r");
}
}
void Data_logging::add_header_name(void)
{
bool init = true;
uint16_t i;
init &= console_.write("time");
put_r_or_n(0);
for (i = 0; i < data_logging_count_; i++)
{
data_logging_entry_t* param = &data_log_[i];
init &= console_.write(reinterpret_cast<uint8_t*>(param->param_name), strlen(param->param_name));
if (!init)
{
break;
if (debug_)
{
print_util_dbg_print("Error appending header!\r\n");
}
}
else
{
put_r_or_n(i);
}
}
file_init_ = init;
}
bool stabilisation_copter_init(stabilisation_copter_t* stabilisation_copter, stabilisation_copter_conf_t* stabiliser_conf, control_command_t* controls, const imu_t* imu, const ahrs_t* ahrs, const position_estimation_t* pos_est,servos_t* servos)
{
bool init_success = true;
//init dependencies
stabilisation_copter->stabiliser_stack = stabiliser_conf->stabiliser_stack;
stabilisation_copter->motor_layout = stabiliser_conf->motor_layout;
stabilisation_copter->controls = controls;
stabilisation_copter->imu = imu;
stabilisation_copter->ahrs = ahrs;
stabilisation_copter->pos_est = pos_est;
stabilisation_copter->servos = servos;
//init controller
controls->control_mode = ATTITUDE_COMMAND_MODE;
controls->yaw_mode = YAW_RELATIVE;
controls->rpy[ROLL] = 0.0f;
controls->rpy[PITCH] = 0.0f;
controls->rpy[YAW] = 0.0f;
controls->tvel[X] = 0.0f;
controls->tvel[Y] = 0.0f;
controls->tvel[Z] = 0.0f;
controls->theading = 0.0f;
controls->thrust = -1.0f;
stabilisation_copter->thrust_hover_point = stabiliser_conf->thrust_hover_point;
print_util_dbg_print("[STABILISATION COPTER] initalised.\r\n");
return init_success;
}
void Pwm_stm32::write_channel(void)
{
//select the output period
if (TIM_ARR(timer_) > period_)
{
print_util_dbg_print("should keep slower period.\r\n");
}
else
{
TIM_ARR(timer_) = period_;
}
if(channel_id_ == PWM_STM32_CHANNEL_1)
{
//select duty cycle
TIM_CCR1(timer_) = duty_cyle_;
}
else if(channel_id_ == PWM_STM32_CHANNEL_2)
{
//select duty cycle
TIM_CCR2(timer_) = duty_cyle_;
}
else if(channel_id_ == PWM_STM32_CHANNEL_3)
{
//select duty cycle
TIM_CCR3(timer_) = duty_cyle_;
}
else if(channel_id_ == PWM_STM32_CHANNEL_4)
{
//select duty cycle
TIM_CCR4(timer_) = duty_cyle_;
}
}
static void data_logging_add_header_name(data_logging_t* data_logging)
{
bool init = false;
uint16_t i;
data_logging_set_t* data_set = data_logging->data_logging_set;
for (i = 0; i < data_set->data_logging_count; i++)
{
data_logging_entry_t* param = &data_set->data_log[i];
int32_t res = f_printf(&data_logging->fil,param->param_name);
if (res == EOF)
{
if (data_logging->debug)
{
print_util_dbg_print("Error appending header!\r\n");
}
init = false;
}
else
{
init = true;
data_logging_put_r_or_n(data_logging, i);
}
}
data_logging->file_init = init;
}
bool servo_mix_ywing_init(servo_mix_ywing_t* mix, const servo_mix_ywing_conf_t* config, const torque_command_t* torque_command, const thrust_command_t* thrust_command, Servo* motor, Servo* flap_top, Servo* flap_right, Servo* flap_left)
{
bool init_success = true;
// Init dependencies
mix->torque_command = torque_command;
mix->thrust_command = thrust_command;
mix->motor = motor;
mix->flap_top = flap_top;
mix->flap_right = flap_right;
mix->flap_left = flap_left;
// Init parameters
mix->flap_top_dir = config->flap_top_dir;
mix->flap_right_dir = config->flap_right_dir;
mix->flap_left_dir = config->flap_left_dir;
mix->min_thrust = config->min_thrust;
mix->max_thrust = config->max_thrust;
mix->min_deflection = config->min_deflection;
mix->max_deflection = config->max_deflection;
print_util_dbg_print("[SERVOS MIX YWING] initialised \r\n");
return init_success;
}
bool state_init(state_t *state, state_t* state_config, const analog_monitor_t* analog_monitor)
{
bool init_success = true;
// Init dependencies
state->analog_monitor = analog_monitor;
// Init parameters
state->autopilot_type = state_config->autopilot_type;
state->autopilot_name = state_config->autopilot_name;
state->mav_state = state_config->mav_state;
state->mav_mode = state_config->mav_mode;
state->source_mode = state_config->source_mode;
state->mav_mode_custom = CUSTOM_BASE_MODE;
state->simulation_mode = state_config->simulation_mode;
init_success &= battery_init(&state->battery,state_config->battery.type,state_config->battery.low_level_limit);
if (state->simulation_mode == HIL_ON)
{
// state->mav_mode |= MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_ON;
}
else
{
// state->mav_mode |= !MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_OFF;
}
state->fence_1_xy = state_config->fence_1_xy;
state->fence_1_z = state_config->fence_1_z;
state->fence_2_xy = state_config->fence_2_xy;
state->fence_2_z = state_config->fence_2_z;
state->out_of_fence_1 = false;
state->out_of_fence_2 = false;
state->nav_plan_active = false;
state->in_the_air = false;
state->reset_position = false;
state->last_heartbeat_msg = time_keeper_get_time();
state->max_lost_connection = state_config->max_lost_connection;
state->connection_lost = false;
state->first_connection_set = false;
state->msg_count = 0;
state->remote_active = state_config->remote_active;
print_util_dbg_print("[STATE] Initialized.\r\n");
return init_success;
}
void lsm330dlc_init(void)
{
if(twim_probe(&AVR32_TWIM0, LSM330_ACC_SLAVE_ADDRESS) == STATUS_OK)
{
print_util_dbg_print("LSM330 sensor found (0x18) \r\n");
}
else
{
print_util_dbg_print("LSM330 sensor not responding (0x18) \r\n");
return;
}
lsm330dlc_acc_init();
lsm330dlc_gyro_init();
lsm330dlc_get_acc_config();
lsm330dlc_get_gyro_config();
}
void simulation_switch_from_reality_to_simulation(simulation_model_t *sim)
{
print_util_dbg_print("Switching from reality to simulation.\r\n");
simulation_reset_simulation(sim);
simulation_calib_set(sim);
sim->pos_est->init_gps_position = false;
}
void position_estimation_reset_home_altitude(position_estimator_t *pos_est)
{
int32_t i;
// reset origin to position where quad is armed if we have GPS
if (pos_est->init_gps_position)
{
pos_est->local_position.origin.longitude = pos_est->gps->longitude;
pos_est->local_position.origin.latitude = pos_est->gps->latitude;
pos_est->local_position.origin.altitude = pos_est->gps->altitude;
pos_est->local_position.timestamp_ms = pos_est->gps->time_last_msg;
pos_est->last_gps_pos = pos_est->local_position;
}
//else
//{
//pos_est->local_position.origin.longitude = HOME_LONGITUDE;
//pos_est->local_position.origin.latitude = HOME_LATITUDE;
//pos_est->local_position.origin.altitude = HOME_ALTITUDE;
//}
// reset barometer offset
bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude);
//pos_est->barometer->altitude_offset = -pos_est->barometer->altitude - pos_est->local_position.pos[2] + pos_est->local_position.origin.altitude;
pos_est->init_barometer = true;
print_util_dbg_print("Offset of the barometer set to the GPS altitude, offset value of:");
print_util_dbg_print_num(pos_est->barometer->altitude_offset,10);
print_util_dbg_print(" = -");
print_util_dbg_print_num(pos_est->barometer->altitude,10);
print_util_dbg_print(" - ");
print_util_dbg_print_num(pos_est->local_position.pos[2],10);
print_util_dbg_print(" + ");
print_util_dbg_print_num(pos_est->local_position.origin.altitude,10);
print_util_dbg_print("\r\n");
// reset position estimator
pos_est->last_alt = 0;
for(i = 0;i < 3;i++)
{
pos_est->last_vel[i] = 0.0f;
pos_est->local_position.pos[i] = 0.0f;
pos_est->vel[i] = 0.0f;
pos_est->vel_bf[i] = 0.0f;
}
}
void print_util_dbg_sep(char c)
{
for (uint8_t i = 0; i < 80; ++i)
{
deb_stream->put(deb_stream->data, c);
}
print_util_dbg_print("\r\n");
}
bool fat_fs_mounting_mount(fat_fs_mounting_t* fat_fs_mounting, bool debug)
{
bool success = false;
FRESULT fr = FR_NO_FILESYSTEM;
if (!fat_fs_mounting->sys_mounted)
{
if ((fr != FR_OK) && (fat_fs_mounting->loop_count < 10))
{
fat_fs_mounting->loop_count += 1;
}
if (fat_fs_mounting->loop_count < 10)
{
fr = f_mount(&fat_fs_mounting->fs, "1:", 1);
//fr = f_mount(&fat_fs_mounting->fs, 0, 1);
if (fr == FR_OK)
{
fat_fs_mounting->sys_mounted = true;
success = true;
}
else
{
fat_fs_mounting->sys_mounted = false;
success = false;
}
if (debug)
{
if (fr == FR_OK)
{
print_util_dbg_print("[FAT] SD card mounted\r\n");
}
else
{
print_util_dbg_print("[FAT] [ERROR] Mounting");
fat_fs_mounting_print_error_signification(fr);
}
}
}
}
return success;
}
bool Data_logging::add_field(const double* val, const char* param_name, uint32_t precision)
{
bool add_success = true;
if (val == NULL)
{
print_util_dbg_print("[DATA LOGGING] Error: Null pointer!");
add_success &= false;
}
else
{
if (data_logging_count_ < config_.max_data_logging_count)
{
if (strlen(param_name) < MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN)
{
data_logging_entry_t* new_param = &data_log_[data_logging_count_];
new_param->param = val;
strcpy(new_param->param_name, param_name);
new_param->data_type = MAVLINK_TYPE_DOUBLE;
new_param->precision = precision;
data_logging_count_ += 1;
add_success &= true;
}
else
{
print_util_dbg_print("[DATA LOGGING] Error: Param name too long.\r\n");
add_success &= false;
}
}
else
{
print_util_dbg_print("[DATA LOGGING] Error: Cannot add more logging param.\r\n");
add_success &= false;
}
}
return add_success;
}
