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");
}
}
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");
}
}
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 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 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(")");
}
}
}
void neighbors_collision_log(neighbors_t *neighbors)
{
uint8_t ind, i;
float relative_position[3];
float dist;
for (ind = 0; ind < neighbors->number_of_neighbors; ind++)
{
for (i = 0; i < 3; i++)
{
relative_position[i] = neighbors->position_estimator->local_position.pos[i] - neighbors->neighbors_list[ind].extrapolated_position[i];
}
dist = vectors_norm_sqr(relative_position);
if (dist < neighbors->collision_dist_sqr && !neighbors->collision_log.collision_flag[ind])
{
neighbors->collision_log.count_collision++;
if (neighbors->collision_log.count_near_miss != 0)
{
neighbors->collision_log.count_near_miss--;
}
neighbors->collision_log.collision_flag[ind] = true;
print_util_dbg_print("Collision with neighbor:");
print_util_dbg_print_num(ind,10);
print_util_dbg_print(", nb of collisions:");
print_util_dbg_print_num(neighbors->collision_log.count_collision,10);
print_util_dbg_print("\r\n");
}
else if (dist < neighbors->near_miss_dist_sqr && !neighbors->collision_log.near_miss_flag[ind])
{
neighbors->collision_log.count_near_miss++;
neighbors->collision_log.near_miss_flag[ind] = true;
print_util_dbg_print("Near miss with neighbor:");
print_util_dbg_print_num(ind,10);
print_util_dbg_print(", nb of near miss:");
print_util_dbg_print_num(neighbors->collision_log.count_near_miss,10);
print_util_dbg_print("\r\n");
}
else if (dist > neighbors->near_miss_dist_sqr)
{
neighbors->collision_log.collision_flag[ind] = false;
neighbors->collision_log.near_miss_flag[ind] = false;
}
}
}
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");
}
int32_t get_joystick_status(int32_t joystick_file_descriptor, int32_t *axes, int32_t *buttons, int32_t axes_size, int32_t buttons_size)
{
int32_t rc;
struct joystick_event event;
if (joystick_file_descriptor < 0)
return -1;
while ((rc = read_joystick_event(joystick_file_descriptor, &event) == 1)) {
event.type &= ~JS_INIT;
if ((event.type == JS_AXIS) && (event.number>=0) &&(event.number<axes_size)) {
axes[event.number] = event.value;
} else if (event.type == JS_BUTTON) {
if ((event.number < buttons_size)&& (event.number>=0)) {
switch (event.value) {
case 0:buttons[event.number] = event.value; break;
case 1:
print_util_dbg_print_num(event.number, 10);print_util_dbg_print(": ");print_util_dbg_print_num(event.value, 10);print_util_dbg_print("\n");
buttons[event.number] = event.value;
switch (event.number) {
case JOY_SAFETY_OFF_BUTTON: axes[RC_SAFETY]=-32000; break;
case JOY_SAFETY_ON_BUTTON: axes[RC_SAFETY]= 32000; break;
case JOY_MODE_1_BUTTON: axes[RC_ID_MODE]=-32000; break;
case JOY_MODE_2_BUTTON: axes[RC_ID_MODE]= 0; break;
case JOY_MODE_3_BUTTON: axes[RC_ID_MODE]= 32000; break;
}
break;
default:
break;
}
}
}
}
return 0;
}
void neighbors_selection_update_consensus_altitude(neighbors_t *neighbors, bool const reset_position)
{
if (reset_position)
{
float new_alt;
print_util_dbg_print("Old altitude:");
print_util_dbg_print_num(neighbors->position_estimator->local_position.origin.altitude,10);
// Mean of own altitude with consensus of neighbors
new_alt = (neighbors->position_estimator->local_position.origin.altitude + neighbors->number_of_neighbors * neighbors->alt_consensus)/(neighbors->number_of_neighbors+1);
neighbors->position_estimator->local_position.pos[2] = 0.0;
//neighbors->gps->alt_consensus_offset += new_alt - neighbors->position_estimator->local_position.origin.altitude;
//neighbors->barometer->alt_consensus_offset += (new_alt - neighbors->position_estimator->local_position.origin.altitude);
neighbors->position_estimator->local_position.origin.altitude = new_alt;
print_util_dbg_print(", New altitude:");
print_util_dbg_print_num(neighbors->position_estimator->local_position.origin.altitude,10);
print_util_dbg_print(", consensus (x100):");
print_util_dbg_print_num(neighbors->alt_consensus*100,10);
print_util_dbg_print(", alt_cons_off (x100):");
//print_util_dbg_print_num(neighbors->gps->alt_consensus_offset *100,10);
print_util_dbg_print("\r\n");
}
}
void mavlink_message_handler_cmd_default_dbg(mavlink_command_long_t* cmd)
{
print_util_dbg_print("Received command with ID");
print_util_dbg_print_num(cmd->command,10);
print_util_dbg_print(" with parameters: [");
print_util_dbg_print_num(cmd->param1,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param2,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param3,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param4,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param5,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param6,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(cmd->param7,10);
print_util_dbg_print("] confirmation: ");
print_util_dbg_print_num(cmd->confirmation,10);
print_util_dbg_print("\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;
}
static void simulation_reset_simulation(simulation_model_t *sim)
{
for (int32_t i = 0; i < 3; i++)
{
sim->rates_bf[i] = 0.0f;
sim->torques_bf[i] = 0.0f;
sim->lin_forces_bf[i] = 0.0f;
sim->vel_bf[i] = 0.0f;
sim->vel[i] = 0.0f;
}
sim->local_position = sim->pos_est->local_position;
sim->ahrs = *sim->estimated_attitude;
print_util_dbg_print("(Re)setting simulation. Origin: (");
print_util_dbg_print_num(sim->pos_est->local_position.origin.latitude*10000000,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->pos_est->local_position.origin.longitude*10000000,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->pos_est->local_position.origin.altitude*1000,10);
print_util_dbg_print("), Position: (x1000) (");
print_util_dbg_print_num(sim->pos_est->local_position.pos[0]*1000,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->pos_est->local_position.pos[1]*1000,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->pos_est->local_position.pos[2]*1000,10);
print_util_dbg_print(")\r\n");
//sim->local_position.origin.latitude = HOME_LATITUDE;
//sim->local_position.origin.longitude = HOME_LONGITUDE;
//sim->local_position.origin.altitude = HOME_ALTITUDE;
//sim->local_position.origin = sim->pos_est->local_position.origin;
//sim->local_position.heading = sim->pos_est->local_position.heading;
}
void data_logging_create_new_log_file(data_logging_t* data_logging, const char* file_name)
{
int32_t i = 0;
char *file_add = malloc(data_logging->buffer_add_size);
snprintf(data_logging->file_name, data_logging->buffer_name_size, "%s", file_name);
data_logging->file_name_init = true;
if (data_logging->log_data)
{
do
{
if (i > 0)
{
if (snprintf(data_logging->name_n_extension, data_logging->buffer_name_size, "%s%s.txt", file_name, file_add) >= data_logging->buffer_name_size)
{
print_util_dbg_print("Name error: The name is too long! It should be, with the extension, maximum ");
print_util_dbg_print_num(data_logging->buffer_name_size,10);
print_util_dbg_print(" and it is ");
print_util_dbg_print_num(sizeof(file_name),10);
print_util_dbg_print("\r\n");
}
}
else
{
if (snprintf(data_logging->name_n_extension, data_logging->buffer_name_size, "%s.txt", file_name) >= data_logging->buffer_name_size)
{
print_util_dbg_print("Name error: The name is too long! It should be maximum ");
print_util_dbg_print_num(data_logging->buffer_name_size,10);
print_util_dbg_print(" characters and it is ");
print_util_dbg_print_num(sizeof(file_name),10);
print_util_dbg_print(" characters.\r\n");
}
}
data_logging->fr = f_open(&data_logging->fil, data_logging->name_n_extension, FA_WRITE | FA_CREATE_NEW);
if (data_logging->debug)
{
print_util_dbg_print("f_open result:");
data_logging_print_error_signification(data_logging);
}
++i;
if (data_logging->fr == FR_EXIST)
{
if(snprintf(file_add,data_logging->buffer_add_size,"_%ld",i) >= data_logging->buffer_add_size)
{
print_util_dbg_print("Error file extension! Extension too long.\r\n");
}
}
//}while((i < data_logging->data_logging_set->max_data_logging_count)&&(data_logging->fr != FR_OK)&&(data_logging->fr != FR_NOT_READY));
} while( (i < data_logging->data_logging_set->max_data_logging_count) && (data_logging->fr == FR_EXIST) );
if (data_logging->fr == FR_OK)
{
data_logging_f_seek(data_logging);
}
if (data_logging->fr == FR_OK)
{
data_logging->file_opened = true;
if (data_logging->debug)
{
print_util_dbg_print("File ");
print_util_dbg_print(data_logging->name_n_extension);
print_util_dbg_print(" opened. \r\n");
}
} //end of if data_logging->fr == FR_OK
}//end of if (data_logging->log_data)
}
void forces_from_servos_diag_quad(simulation_model_t* sim)
{
float motor_command[4];
float rotor_lifts[4], rotor_drags[4], rotor_inertia[4];
float ldb;
quat_t wind_gf = {.s = 0, .v = {sim->vehicle_config.wind_x, sim->vehicle_config.wind_y, 0.0f}};
quat_t wind_bf = quaternions_global_to_local(sim->ahrs.qe, wind_gf);
float sqr_lateral_airspeed = SQR(sim->vel_bf[0] + wind_bf.v[0]) + SQR(sim->vel_bf[1] + wind_bf.v[1]);
float lateral_airspeed = sqrt(sqr_lateral_airspeed);
float old_rotor_speed;
for (int32_t i = 0; i < 4; i++)
{
motor_command[i] = (float)sim->mix->servos->servo[i].value - sim->vehicle_config.rotor_rpm_offset;
if (motor_command[i] < 0.0f)
{
motor_command[i] = 0;
}
// temporarily save old rotor speeds
old_rotor_speed = sim->rotorspeeds[i];
// estimate rotor speeds by low - pass filtering
//sim->rotorspeeds[i] = (sim->vehicle_config.rotor_lpf) * sim->rotorspeeds[i] + (1.0f - sim->vehicle_config.rotor_lpf) * (motor_command[i] * sim->vehicle_config.rotor_rpm_gain);
sim->rotorspeeds[i] = (motor_command[i] * sim->vehicle_config.rotor_rpm_gain);
// calculate torque created by rotor inertia
rotor_inertia[i] = (sim->rotorspeeds[i] - old_rotor_speed) / sim->dt * sim->vehicle_config.rotor_momentum;
ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, -sim->vel_bf[Z]);
//ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, 0.0f);
rotor_lifts[i] = ldb * sim->vehicle_config.rotor_cl;
rotor_drags[i] = ldb * sim->vehicle_config.rotor_cd;
}
float mpos_x = sim->vehicle_config.rotor_arm_length / 1.4142f;
float mpos_y = sim->vehicle_config.rotor_arm_length / 1.4142f;
// torque around x axis (roll)
sim->torques_bf[ROLL] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_rear_left] )
- (rotor_lifts[sim->mix->motor_front_right] + rotor_lifts[sim->mix->motor_rear_right] )) * mpos_y;;
// torque around y axis (pitch)
sim->torques_bf[PITCH] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_front_right] )
- (rotor_lifts[sim->mix->motor_rear_left] + rotor_lifts[sim->mix->motor_rear_right] ))* mpos_x;
sim->torques_bf[YAW] = (sim->mix->motor_front_left_dir * (10.0f * rotor_drags[sim->mix->motor_front_left] + rotor_inertia[sim->mix->motor_front_left])
+ sim->mix->motor_front_right_dir * (10.0f * rotor_drags[sim->mix->motor_front_right] + rotor_inertia[sim->mix->motor_front_right])
+ sim->mix->motor_rear_left_dir * (10.0f * rotor_drags[sim->mix->motor_rear_left] + rotor_inertia[sim->mix->motor_rear_left])
+ sim->mix->motor_rear_right_dir * (10.0f * rotor_drags[sim->mix->motor_rear_right] + rotor_inertia[sim->mix->motor_rear_right] ))* sim->vehicle_config.rotor_diameter;
sim->lin_forces_bf[X] = -(sim->vel_bf[X] - wind_bf.v[0]) * lateral_airspeed * sim->vehicle_config.vehicle_drag;
sim->lin_forces_bf[Y] = -(sim->vel_bf[Y] - wind_bf.v[1]) * lateral_airspeed * sim->vehicle_config.vehicle_drag;
sim->lin_forces_bf[Z] = -(rotor_lifts[sim->mix->motor_front_left]+ rotor_lifts[sim->mix->motor_front_right] +rotor_lifts[sim->mix->motor_rear_left] +rotor_lifts[sim->mix->motor_rear_right]);
}
//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------
bool simulation_init(simulation_model_t* sim, const simulation_config_t* sim_config, ahrs_t* ahrs, imu_t* imu, position_estimation_t* pos_est, barometer_t* pressure, gps_t* gps, sonar_t* sonar, state_t* state, bool* waypoint_set, const servo_mix_quadcotper_diag_t* mix)
{
bool init_success = true;
//Init dependencies
sim->vehicle_config = *sim_config;
sim->imu = imu;
sim->pos_est = pos_est;
sim->pressure = pressure;
sim->gps = gps;
sim->sonar = sonar;
sim->state = state;
sim->nav_plan_active = &state->nav_plan_active;
sim->mix = mix;
// set initial conditions to a given attitude_filter
sim->estimated_attitude = ahrs;
sim->ahrs = *ahrs;
print_util_dbg_print("Attitude:");
print_util_dbg_print_num(sim->ahrs.qe.s*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[0]*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[1]*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[2]*100,10);
print_util_dbg_print("\r\n");
for (int32_t i = 0; i < ROTORCOUNT; i++)
{
sim->rotorspeeds[i] = 0.0f;
}
sim->last_update = time_keeper_get_micros();
sim->dt = 0.01f;
simulation_reset_simulation(sim);
simulation_calib_set(sim);
print_util_dbg_print("[HIL SIMULATION] initialised.\r\n");
return init_success;
}
bool Data_logging::open_new_log_file(void)
{
bool create_success = true;
uint32_t i = 0;
if (log_data_)
{
do
{
// Create flag for successfully written file names
bool successful_filename = true;
// Add iteration number to name_n_extension_ (does not yet have extension)
successful_filename &= filename_append_int(name_n_extension_, file_name_, i, buffer_name_size_);
// Add extension (.txt) to name_n_extension_
successful_filename &= filename_append_extension(name_n_extension_, name_n_extension_, buffer_name_size_);
// Check if there wasn't enough memory allocated to name_n_extension_
if (!successful_filename)
{
print_util_dbg_print("Name error: The name is too long! It should be, with the extension, maximum ");
print_util_dbg_print_num(buffer_name_size_, 10);
print_util_dbg_print(" and it is ");
print_util_dbg_print_num(sizeof(name_n_extension_), 10);
print_util_dbg_print("\r\n");
create_success = false;
}
// If the filename was successfully created, try to open a file
if (successful_filename)
{
int8_t exists = console_.get_stream()->exists(name_n_extension_);
switch (exists)
{
case -1:
sys_status_ = false;
create_success = false;
break;
case 0:
sys_status_ = true;
create_success = console_.get_stream()->open(name_n_extension_);
break;
case 1:
sys_status_ = true;
create_success = false;
break;
}
}
if (debug_)
{
print_util_dbg_print("Open result:");
print_util_dbg_print_num(create_success, 10);
print_util_dbg_print("\r\n");
}
++i;
}
while ((i < config_.max_logs) && (!create_success) && sys_status_);
if (create_success)
{
seek();
file_opened_ = true;
if (debug_)
{
print_util_dbg_print("File ");
print_util_dbg_print(name_n_extension_);
print_util_dbg_print(" opened. \r\n");
}
} //end of if fr == FR_OK
}//end of if (log_data_)
return create_success;
}
void mavlink_message_handler_receive(mavlink_message_handler_t* message_handler, mavlink_received_t* rec)
{
mavlink_message_t* msg = &rec->msg;
if ( message_handler->debug )
{
mavlink_message_handler_msg_default_dbg(msg);
}
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
{
// The message is a command
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
//print packet command and parameters for debug
print_util_dbg_print("target sysID:");
print_util_dbg_print_num(cmd.target_system,10);
print_util_dbg_print(", target compID:");
print_util_dbg_print_num(cmd.target_component,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("parameters: ");
print_util_dbg_print_num(cmd.param1,10);
print_util_dbg_print_num(cmd.param2,10);
print_util_dbg_print_num(cmd.param3,10);
print_util_dbg_print_num(cmd.param4,10);
print_util_dbg_print_num(cmd.param5,10);
print_util_dbg_print_num(cmd.param6,10);
print_util_dbg_print_num(cmd.param7,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("command id:");
print_util_dbg_print_num(cmd.command,10);
print_util_dbg_print(", confirmation:");
print_util_dbg_print_num(cmd.confirmation,10);
print_util_dbg_print("\r\n");
if (cmd.command >= 0 && cmd.command < MAV_CMD_ENUM_END)
{
// The command has valid command ID
if( (cmd.target_system == message_handler->mavlink_stream->sysid)||(cmd.target_system == MAV_SYS_ID_ALL) )
{
mav_result_t result = MAV_RESULT_UNSUPPORTED;
// The command is for this system
for (uint32_t i = 0; i < message_handler->cmd_callback_set->callback_count; ++i)
{
if ( match_cmd(message_handler, &message_handler->cmd_callback_set->callback_list[i], msg, &cmd) )
{
mavlink_cmd_callback_function_t function = message_handler->cmd_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->cmd_callback_set->callback_list[i].module_struct;
// Call appropriate function callback
result = function(module_struct, &cmd);
break;
}
}
// Send acknowledgment message
mavlink_message_t msg;
mavlink_msg_command_ack_pack( message_handler->mavlink_stream->sysid,
message_handler->mavlink_stream->compid,
&msg,
cmd.command,
result);
mavlink_stream_send(message_handler->mavlink_stream, &msg);
}
}
}
else if ( msg->msgid >= 0 && msg->msgid < MAV_MSG_ENUM_END )
{
// The message has a valid message ID, and is not a command
for (uint32_t i = 0; i < message_handler->msg_callback_set->callback_count; ++i)
{
if ( match_msg(message_handler, &message_handler->msg_callback_set->callback_list[i], msg) )
{
mavlink_msg_callback_function_t function = message_handler->msg_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->msg_callback_set->callback_list[i].module_struct;
uint32_t sys_id = *message_handler->msg_callback_set->callback_list[i].sys_id;
// Call appropriate function callback
function(module_struct, sys_id, msg);
}
}
}
}
