int16_t spektrum_satellite_get_channel(uint8_t index) {
int32_t i;
if (time_keeper_get_millis()-last_update>20)
{
#ifdef KEYBOARD_ACTIVE
get_keyboard_input(&joystick_axes);
#endif
get_joystick_status(joystick_filedescriptor, &joystick_axes, &joystick_buttons, 16, 16);
for (i=0; i<16; i++) {
if (joystick_axes[i]>joy_max[i]) joy_max[i]=joystick_axes[i];
if (joystick_axes[i]<joy_min[i]) joy_min[i]=joystick_axes[i];
}
sat.channels[RC_ROLL] = joystick_axes[JOY_ROLL]*J_GAIN/ (joy_max[JOY_ROLL]-joy_min[JOY_ROLL]);
sat.channels[RC_PITCH] = joystick_axes[JOY_PITCH]*J_GAIN/ (joy_max[JOY_PITCH]-joy_min[JOY_PITCH]);
sat.channels[RC_YAW] = joystick_axes[JOY_YAW]*J_GAIN/ (joy_max[JOY_YAW]-joy_min[JOY_YAW]);
sat.channels[RC_THROTTLE] = -joystick_axes[JOY_THROTTLE]*J_GAIN/ (joy_max[JOY_THROTTLE]-joy_min[JOY_THROTTLE]);
sat.channels[RC_SAFETY] = joystick_axes[JOY_SAFETY] *J_GAIN/ (joy_max[JOY_SAFETY]-joy_min[JOY_SAFETY]);
sat.channels[RC_ID_MODE] =joystick_axes[JOY_ID_MODE]*J_GAIN/ (joy_max[JOY_ID_MODE]-joy_min[JOY_ID_MODE]);
last_update=time_keeper_get_millis();
}
return sat.channels[index];
}
void simulation_fake_gps_fix(simulation_model_t* sim, uint32_t timestamp_ms)
{
local_coordinates_t fake_pos;
fake_pos.pos[X] = 10.0f;
fake_pos.pos[Y] = 10.0f;
fake_pos.pos[Z] = 0.0f;
fake_pos.origin.latitude = sim->vehicle_config.home_coordinates[0];
fake_pos.origin.longitude = sim->vehicle_config.home_coordinates[1];
fake_pos.origin.altitude = sim->vehicle_config.home_coordinates[2];
fake_pos.timestamp_ms = timestamp_ms;
global_position_t gpos = coord_conventions_local_to_global_position(fake_pos);
sim->gps->latitude = gpos.latitude;
sim->gps->longitude = gpos.longitude;
sim->gps->altitude = gpos.altitude;
sim->gps->time_last_msg = time_keeper_get_millis();
sim->gps->time_last_posllh_msg = time_keeper_get_millis();
sim->gps->time_last_velned_msg = time_keeper_get_millis();
sim->gps->north_speed = 0.0f;
sim->gps->east_speed = 0.0f;
sim->gps->vertical_speed = 0.0f;
sim->gps->status = GPS_OK;
sim->gps->healthy = true;
}
task_return_t scheduler_send_rt_stats(scheduler_t* scheduler)
{
const mavlink_stream_t* mavlink_stream = scheduler->mavlink_stream;
task_entry_t* stab_task = scheduler_get_task_by_id(scheduler,0);
mavlink_message_t msg;
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabAvgDelay",
stab_task->delay_avg);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabDelayVar",
sqrt(stab_task->delay_var_squared));
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabMaxDelay",
stab_task->delay_max);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabRTvio",
stab_task->rt_violations);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabExTime",
stab_task->execution_time);
mavlink_stream_send(mavlink_stream, &msg);
stab_task->rt_violations = 0;
stab_task->delay_max = 0;
return TASK_RUN_SUCCESS;
}
void get_keyboard_input(int32_t *joystick_axes) {
if (!kbhit()) {
if (time_keeper_get_millis()-last_keypress<1000) {
if (time_keeper_get_millis()-last_keypress<500) {
joystick_axes[JOY_PITCH]/=2;
joystick_axes[JOY_ROLL]/=2;
joystick_axes[JOY_YAW]/=2;
joystick_axes[JOY_THROTTLE]/=2;
} else {
joystick_axes[JOY_PITCH]=0;
joystick_axes[JOY_ROLL]=0;
joystick_axes[JOY_YAW]=0;
joystick_axes[JOY_THROTTLE]=0;
}
}
} else
while (kbhit()) {
last_keypress=time_keeper_get_millis();
char c=getch();
if (c==3) exit(0);
//wdbg_print(c);
switch (c) {
case 'i': joystick_axes[JOY_PITCH]=-20000; break;
case 'k': joystick_axes[JOY_PITCH]= 20000; break;
case 'j': joystick_axes[JOY_ROLL]=-20000; break;
case 'l': joystick_axes[JOY_ROLL]= 20000; break;
case 'a': joystick_axes[JOY_YAW]=-20000; break;
case 'd': joystick_axes[JOY_YAW]= 20000; break;
case 'w': joystick_axes[JOY_THROTTLE]=-10000; break;
case 's': joystick_axes[JOY_THROTTLE]= 10000; break;
case 'x': joystick_axes[JOY_SAFETY]=-20000; break;
case 'c': joystick_axes[JOY_SAFETY]= 20000; break;
case 'm': joystick_axes[JOY_THROTTLE]= 32000;
joystick_axes[JOY_YAW]= 32000;
break;
case 'o': joystick_axes[JOY_THROTTLE]= 32000;
joystick_axes[JOY_YAW]= -32000;
break;
case '1': joystick_axes[JOY_ID_MODE]=-20000; break;
case '2': joystick_axes[JOY_ID_MODE]= 0; break;
case '3': joystick_axes[JOY_ID_MODE]= 20000; break;
}
}
}
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 bmp085_telemetry_send_pressure(const barometer_t* bmp085, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_scaled_pressure_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
bmp085->pressure / 100.0f,
bmp085->vario_vz,
bmp085->temperature * 100.0f);
}
void neighbors_compute_communication_frequency(neighbors_t* neighbors)
{
uint16_t i;
if (neighbors->number_of_neighbors != 0)
{
uint32_t actual_time = time_keeper_get_millis();
if ((actual_time - neighbors->previous_time) >= neighbors->update_time_interval)
{
//mavlink_message_t msg;
neighbors->mean_comm_frequency = 0.0f;
neighbors->variance_comm_frequency = 0.0f;
for (i=0;i<neighbors->number_of_neighbors;++i)
{
//mavlink_msg_named_value_int_pack( neighbors->mavlink_stream->sysid,
//neighbors->mavlink_stream->compid,
//&msg,
//time_keeper_get_millis(),
//"msg_count",
//neighbors->neighbors_list[i].msg_count);
//mavlink_stream_send(neighbors->mavlink_stream,&msg);
neighbors->neighbors_list[i].comm_frequency = FREQ_LPF*neighbors->neighbors_list[i].comm_frequency + (1.0f-FREQ_LPF)*neighbors->neighbors_list[i].msg_count*1000.0f/(actual_time - neighbors->previous_time);
neighbors->neighbors_list[i].msg_count = 0;
neighbors->mean_comm_frequency += neighbors->neighbors_list[i].comm_frequency;
}
neighbors->previous_time = actual_time;
neighbors->mean_comm_frequency /= neighbors->number_of_neighbors;
for (i=0;i<neighbors->number_of_neighbors;++i)
{
neighbors->variance_comm_frequency += SQR(neighbors->mean_comm_frequency - neighbors->neighbors_list[i].comm_frequency);
}
neighbors->variance_comm_frequency /= neighbors->number_of_neighbors;
//mavlink_msg_named_value_float_pack( neighbors->mavlink_stream->sysid,
//neighbors->mavlink_stream->compid,
//&msg,
//time_keeper_get_millis(),
//"comm_freq",neighbors->mean_comm_frequency);
//mavlink_stream_send(neighbors->mavlink_stream,&msg);
}
}
else
{
neighbors->mean_comm_frequency = 0.0f;
neighbors->variance_comm_frequency = 0.0f;
}
}
void stabilisation_telemetry_send_rpy_rates_error(const stabiliser_t* stabiliser, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
stabiliser->rpy_controller[0].error,
stabiliser->rpy_controller[1].error,
stabiliser->rpy_controller[2].error,
stabiliser->thrust_controller.error );
}
void stabilisation_telemetry_send_rpy_thrust_setpoint(const control_command_t* controls, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
// Controls output
mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
controls->rpy[ROLL],
controls->rpy[PITCH],
controls->rpy[YAW],
controls->thrust);
}
void position_estimation_telemetry_send_position(const position_estimation_t* pos_est, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_local_position_ned_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
pos_est->local_position.pos[0],
pos_est->local_position.pos[1],
pos_est->local_position.pos[2],
pos_est->vel_bf[0],
pos_est->vel_bf[1],
pos_est->vel_bf[2]);
}
void curvace_telemetry_send(const curvace_t* curvace, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
static uint8_t sensor_id = 0;
int16_t of_azimuth[CURVACE_NB_OF];
int16_t of_elevation[CURVACE_NB_OF];
int16_t azimuth[CURVACE_NB_OF];
int16_t elevation[CURVACE_NB_OF];
uint8_t info[CURVACE_NB_OF];
uint8_t offset;
uint8_t nb_of_per_message = 18;
// fill data in 16bits
for (uint8_t i = 0; i < CURVACE_NB_OF; ++i)
{
of_azimuth[i] = 1000 * curvace->of.all[i].x;
of_elevation[i] = 1000 * curvace->of.all[i].y;
azimuth[i] = 1000 * curvace->roi_coord.all[i].azimuth;
elevation[i] = 1000 * curvace->roi_coord.all[i].elevation;
info[i] = 0;
}
// Send in 6 chunks
offset = sensor_id * nb_of_per_message;
mavlink_msg_spherical_optic_flow_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
sensor_id,
4,
nb_of_per_message,
0,
of_azimuth + offset,
of_elevation + offset,
azimuth + offset,
elevation + offset,
info + offset);
// Increment sensor id
sensor_id = ( sensor_id + 1 ) % 6;
}
void neighbors_selection_init(neighbors_t* neighbors, position_estimator_t *position_estimator, state_t* state, gps_t* gps, barometer_t* barometer, mavlink_message_handler_t *message_handler, const mavlink_stream_t* mavlink_stream)
{
neighbors->number_of_neighbors = 0;
neighbors->position_estimator = position_estimator;
neighbors->mavlink_stream = mavlink_stream;
neighbors->state = state;
neighbors->gps = gps;
neighbors->barometer = barometer;
neighbors->alt_consensus = position_estimator->local_position.origin.altitude;
neighbors->LPF_alt = 0.9;
neighbors->mean_comm_frequency = 0.0f;
neighbors->variance_comm_frequency = 0.0f;
neighbors->previous_time = time_keeper_get_millis();
neighbors->update_time_interval = 1000.0f; // 1 sec
uint8_t i;
neighbors->collision_log.count_near_miss = 0;
neighbors->collision_log.count_collision = 0;
for (i = 0; i < MAX_NUM_NEIGHBORS; i++)
{
neighbors->collision_log.near_miss_flag[i] = false;
neighbors->collision_log.collision_flag[i] = false;
}
neighbors->collision_dist_sqr = SQR(6.0f);
neighbors->near_miss_dist_sqr = SQR(10.0f);
// Add callbacks for onboard parameters requests
mavlink_message_handler_msg_callback_t callback;
callback.message_id = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; // 33
callback.sysid_filter = MAV_SYS_ID_ALL;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &neighbors_selection_read_message_from_neighbors;
callback.module_struct = (handling_module_struct_t) neighbors;
mavlink_message_handler_add_msg_callback( message_handler, &callback );
print_util_dbg_print("Neighbor selection initialized.\r\n");
}
void position_estimation_telemetry_send_global_position(const position_estimation_t* pos_est, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
// send integrated position (for now there is no GPS error correction...!!!)
global_position_t gpos = coord_conventions_local_to_global_position(pos_est->local_position);
//mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg)
mavlink_msg_global_position_int_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
time_keeper_get_millis(),
gpos.latitude * 10000000,
gpos.longitude * 10000000,
gpos.altitude * 1000.0f,
-pos_est->local_position.pos[2] * 1000,
pos_est->vel[0] * 100.0f,
pos_est->vel[1] * 100.0f,
pos_est->vel[2] * 100.0f,
pos_est->local_position.heading);
}
void spektrum_satellite_init (void) {
int32_t i;
for (i=0; i<16; i++) {
sat.channels[i]=-500;
channel_center[i]=0;
joystick_axes[i]=0;
joystick_buttons[i]=0;
joy_max[i]=32700;
joy_min[i]=-32700;
}
sat.channels[RC_THROTTLE]=0;
channel_center[RC_YAW]=0;
joystick_filedescriptor=open_joystick(JOYSTICK_DEVICE);
last_update=time_keeper_get_millis();
#ifdef KEYBOARD_ACTIVE
set_conio_terminal_mode();
#endif
}
task_return_t imu_send_scaled(imu_t* imu)
{
mavlink_message_t msg;
mavlink_msg_scaled_imu_pack(imu->mavlink_stream->sysid,
imu->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
1000 * imu->scaled_accelero.data[IMU_X],
1000 * imu->scaled_accelero.data[IMU_Y],
1000 * imu->scaled_accelero.data[IMU_Z],
1000 * imu->scaled_gyro.data[IMU_X],
1000 * imu->scaled_gyro.data[IMU_Y],
1000 * imu->scaled_gyro.data[IMU_Z],
1000 * imu->scaled_compass.data[IMU_X],
1000 * imu->scaled_compass.data[IMU_Y],
1000 * imu->scaled_compass.data[IMU_Z]);
mavlink_stream_send(imu->mavlink_stream,&msg);
return TASK_RUN_SUCCESS;
}
task_return_t remote_send_raw(const remote_t* remote)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_raw_pack( remote->mavlink_stream->sysid,
remote->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
0,
remote->sat->channels[0] + 1024,
remote->sat->channels[1] + 1024,
remote->sat->channels[2] + 1024,
remote->sat->channels[3] + 1024,
remote->sat->channels[4] + 1024,
remote->sat->channels[5] + 1024,
remote->sat->channels[6] + 1024,
remote->sat->channels[7] + 1024,
// remote->mode.current_desired_mode.byte);
remote->signal_quality );
mavlink_stream_send(remote->mavlink_stream, &msg);
return TASK_RUN_SUCCESS;
}
task_return_t remote_send_scaled(const remote_t* remote)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_scaled_pack( remote->mavlink_stream->sysid,
remote->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
0,
remote->channels[0] * 10000.0f,
remote->channels[1] * 10000.0f,
remote->channels[2] * 10000.0f,
remote->channels[3] * 10000.0f,
remote->channels[4] * 10000.0f,
remote->channels[5] * 10000.0f,
remote->channels[6] * 10000.0f,
remote->channels[7] * 10000.0f,
remote->mode.current_desired_mode.byte );
// remote->signal_quality );
mavlink_stream_send(remote->mavlink_stream, &msg);
return TASK_RUN_SUCCESS;
}
void neighbors_selection_read_message_from_neighbors(neighbors_t *neighbors, uint32_t sysid, mavlink_message_t* msg)
{
//Use this block for message debugging
/*
print_util_dbg_print("\n 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(" for component");
print_util_dbg_print_num(msg->compid,10);
print_util_dbg_print( "\r\n");
*/
uint8_t i;
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg,&packet);
//Check if coming from a neighbor
if (msg->sysid != (uint8_t)sysid)
{
global_position_t global_pos_neighbor;
local_coordinates_t local_pos_neighbor;
uint8_t actual_neighbor;
global_pos_neighbor.longitude = (double)packet.lon / 10000000.0f;
global_pos_neighbor.latitude = (double)packet.lat / 10000000.0f;
global_pos_neighbor.altitude = (float)packet.alt / 1000.0f;
global_pos_neighbor.heading = (float)packet.hdg;
local_pos_neighbor = coord_conventions_global_to_local_position(global_pos_neighbor,neighbors->position_estimator->local_position.origin);
local_pos_neighbor.pos[2] = -packet.relative_alt / 1000.0f;
bool ID_found = false;
i = 0;
while ((!ID_found)&&(i < neighbors->number_of_neighbors))
{
if (msg->sysid == neighbors->neighbors_list[i].neighbor_ID)
{
ID_found = true;
}
else
{
i++;
}
}
if (i >= neighbors->number_of_neighbors)
{
if (neighbors->number_of_neighbors < MAX_NUM_NEIGHBORS)
{
actual_neighbor = neighbors->number_of_neighbors;
neighbors->number_of_neighbors++;
neighbors->neighbors_list[actual_neighbor].comm_frequency = 0.0f;
neighbors->neighbors_list[actual_neighbor].msg_count = 0;
}
else
{
// This case shouldn't happen
print_util_dbg_print("Error! There is more neighbors than planned!\n");
actual_neighbor = neighbors->number_of_neighbors - 1;
}
}
else
{
actual_neighbor = i;
}
if ( (neighbors->state->mav_state == MAV_STATE_STANDBY)||(neighbors->state->mav_state == MAV_STATE_BOOT) )
{
neighbors->alt_consensus = neighbors->LPF_alt * neighbors->alt_consensus + (1.0f - neighbors->LPF_alt) * global_pos_neighbor.altitude;
// print_util_dbg_print("Alt neighbor (x1000):");
// print_util_dbg_print_num(global_pos_neighbor.altitude*1000.0f,10);
// print_util_dbg_print(", consensus alt (x100):");
// print_util_dbg_print_num(neighbors->alt_consensus*100.0f,10);
// print_util_dbg_print(", alt neighbor(x100):");
// print_util_dbg_print_num(local_pos_neighbor.pos[2]*100.0f,10);
// print_util_dbg_print(", own local alt(x100):");
// print_util_dbg_print_num(neighbors->position_estimator->local_position.pos[2]*100.0f,10);
// print_util_dbg_print("\r\n");
// delay_ms(100);
}
neighbors->neighbors_list[actual_neighbor].neighbor_ID = msg->sysid;
for (i = 0; i < 3; i++)
{
neighbors->neighbors_list[actual_neighbor].position[i] = local_pos_neighbor.pos[i];
}
neighbors->neighbors_list[actual_neighbor].velocity[X] = packet.vx / 100.0f;
neighbors->neighbors_list[actual_neighbor].velocity[Y] = packet.vy / 100.0f;
neighbors->neighbors_list[actual_neighbor].velocity[Z] = packet.vz / 100.0f;
neighbors->neighbors_list[actual_neighbor].size = SIZE_VHC_ORCA;
neighbors->neighbors_list[actual_neighbor].time_msg_received = time_keeper_get_millis();
neighbors->neighbors_list[actual_neighbor].msg_count++;
// print_util_dbg_print("Neighbor with ID ");
// print_util_dbg_print_num(neighbors->neighbors_list[actual_neighbor].neighbor_ID,10);
// print_util_dbg_print(" at position ");
// print_util_dbg_print_vector(neighbors->neighbors_list[actual_neighbor].position,3);
// print_util_dbg_print(" with velocity ");
// delay_ms(100);
// print_util_dbg_print_vector(neighbors->neighbors_list[actual_neighbor].velocity,3);
// print_util_dbg_print(" with relative position ");
// float rel_pos[3];
// uint8_t i;
// for (i = 0; i < 3; i++)
// {
// rel_pos[i] = neighbors->neighbors_list[actual_neighbor].position[i] - neighbors->position_estimator->local_position.pos[i];
// }
// print_util_dbg_print_vector(rel_pos,3);
// print_util_dbg_print(", dist (x100):");
// print_util_dbg_print_num(vectors_norm(rel_pos)*100,10);
// print_util_dbg_print("\r\n");
// delay_ms(100);
}
}
task_return_t data_logging_update(data_logging_t* data_logging)
{
if (data_logging->log_data == 1)
{
if (data_logging->file_opened)
{
if (data_logging->file_init)
{
data_logging->time_ms = time_keeper_get_millis();
if (data_logging->fr == FR_OK)
{
data_logging_log_parameters(data_logging);
}
}
else
{
if (data_logging->fr == FR_OK)
{
data_logging_add_header_name(data_logging);
}
}
} //end of if (data_logging->file_opened)
else
{
if (!data_logging->file_name_init)
{
data_logging->file_name = "Default";
}
if ((data_logging->fr != FR_OK)&&(data_logging->loop_count < 10))
{
data_logging->loop_count += 1;
}
if (data_logging->loop_count < 10)
{
data_logging->fr = f_mount(&data_logging->fs,"",1);
if (data_logging->fr == FR_OK)
{
data_logging->sys_mounted = true;
data_logging_create_new_log_file(data_logging,data_logging->file_name);
}
else
{
data_logging->sys_mounted = false;
}
}
}//end of else !data_logging->file_opened
}//end of if (data_logging->log_data == 1)
else
{
data_logging->loop_count = 0;
if (data_logging->file_opened)
{
if (data_logging->fr != FR_NO_FILE)
{
bool succeed = false;
for (uint8_t i = 0; i < 5; ++i)
{
if (data_logging->debug)
{
print_util_dbg_print("Attempt to close file\r\n");
}
data_logging->fr = f_close(&data_logging->fil);
if ( data_logging->fr == FR_OK)
{
succeed = true;
break;
}
if(data_logging->fr == FR_NO_FILE)
{
break;
}
} //end for loop
data_logging->file_opened = false;
if (data_logging->debug)
{
if ( succeed)
{
print_util_dbg_print("File closed\r\n");
}
else
{
print_util_dbg_print("Error closing file\r\n");
}
}
}//end of if (data_logging->fr != FR_NO_FILE)
else
{
data_logging->file_opened = false;
}
}//end of if (data_logging->file_opened)
if (data_logging->sys_mounted)
{
data_logging->fr = f_mount(&data_logging->fs,"",0);
if (data_logging->debug)
{
print_util_dbg_print("f_(un)mount result:");
data_logging_print_error_signification(data_logging);
}
if (data_logging->fr == FR_OK)
{
data_logging->file_opened = false;
data_logging->sys_mounted = false;
}
}
}//end of else (data_logging->log_data != 1)
return TASK_RUN_SUCCESS;
}
void data_logging_init(data_logging_t* data_logging, const data_logging_conf_t* config)
{
// Init debug mode
data_logging->debug = config->debug;
// Allocate memory for the onboard data_log
data_logging->data_logging_set = malloc( sizeof(data_logging_set_t) + sizeof(data_logging_entry_t[config->max_data_logging_count]) );
if ( data_logging->data_logging_set != NULL )
{
data_logging->data_logging_set->max_data_logging_count = config->max_data_logging_count;
data_logging->data_logging_set->max_logs = config->max_logs;
data_logging->data_logging_set->log_interval = config->log_interval;
data_logging->data_logging_set->data_logging_count = 0;
}
else
{
print_util_dbg_print("[DATA LOGGING] ERROR ! Bad memory allocation.\r\n");
data_logging->data_logging_set->max_data_logging_count = 0;
data_logging->data_logging_set->max_logs = 0;
data_logging->data_logging_set->log_interval = 0;
data_logging->data_logging_set->data_logging_count = 0;
}
// Automaticly add the time as first logging parameter
data_logging_add_parameter_uint32(data_logging,&data_logging->time_ms,"time");
data_logging->file_init = false;
data_logging->file_opened = false;
data_logging->file_name_init = false;
data_logging->log_data = config->log_data;
data_logging->loop_count = 0;
#if _USE_LFN
data_logging->buffer_name_size = _MAX_LFN;
data_logging->buffer_add_size = _MAX_LFN;
#else
data_logging->buffer_name_size = 8;
data_logging->buffer_add_size = 8;
#endif
data_logging->file_name = malloc(data_logging->buffer_name_size);
data_logging->name_n_extension = malloc(data_logging->buffer_name_size);
data_logging->fr = f_mount(&data_logging->fs, "", 1);
if (data_logging->fr == FR_OK)
{
data_logging->sys_mounted = true;
}
else
{
data_logging->sys_mounted = false;
}
if (data_logging->debug)
{
if (data_logging->fr == FR_OK)
{
print_util_dbg_print("SD card mounted\r\n");
}
else
{
print_util_dbg_print("Mounting error:");
data_logging_print_error_signification(data_logging);
}
}
data_logging->logging_time = time_keeper_get_millis();
print_util_dbg_print("[Data logging] Data logging initialised.\r\n");
}
void simulation_update(simulation_model_t *sim)
{
int32_t i;
quat_t qtmp1, qvel_bf, qed;
const quat_t front = {.s = 0.0f, .v = {1.0f, 0.0f, 0.0f}};
const quat_t up = {.s = 0.0f, .v = {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}};
uint32_t now = time_keeper_get_micros();
sim->dt = (now - sim->last_update) / 1000000.0f;
if (sim->dt > 0.1f)
{
sim->dt = 0.1f;
}
sim->last_update = now;
// compute torques and forces based on servo commands
forces_from_servos_diag_quad(sim);
// integrate torques to get simulated gyro rates (with some damping)
sim->rates_bf[0] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[0] + sim->dt * sim->torques_bf[0] / sim->vehicle_config.roll_pitch_momentum, 10.0f);
sim->rates_bf[1] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[1] + sim->dt * sim->torques_bf[1] / sim->vehicle_config.roll_pitch_momentum, 10.0f);
sim->rates_bf[2] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[2] + sim->dt * sim->torques_bf[2] / sim->vehicle_config.yaw_momentum, 10.0f);
for (i = 0; i < 3; i++)
{
qtmp1.v[i] = 0.5f * sim->rates_bf[i];
}
qtmp1.s = 0;
// apply step rotation
qed = quaternions_multiply(sim->ahrs.qe,qtmp1);
// TODO: correct this formulas when using the right scales
sim->ahrs.qe.s = sim->ahrs.qe.s + qed.s * sim->dt;
sim->ahrs.qe.v[0] += qed.v[0] * sim->dt;
sim->ahrs.qe.v[1] += qed.v[1] * sim->dt;
sim->ahrs.qe.v[2] += qed.v[2] * sim->dt;
sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe);
sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up);
sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front);
// velocity and position integration
// check altitude - if it is lower than 0, clamp everything (this is in NED, assuming negative altitude)
if (sim->local_position.pos[Z] >0)
{
sim->vel[Z] = 0.0f;
sim->local_position.pos[Z] = 0.0f;
// simulate "acceleration" caused by contact force with ground, compensating gravity
for (i = 0; i < 3; i++)
{
sim->lin_forces_bf[i] = sim->ahrs.up_vec.v[i] * sim->vehicle_config.total_mass * sim->vehicle_config.sim_gravity;
}
// slow down... (will make velocity slightly inconsistent until next update cycle, but shouldn't matter much)
for (i = 0; i < 3; i++)
{
sim->vel_bf[i] = 0.95f * sim->vel_bf[i];
}
//upright
sim->rates_bf[0] = sim->ahrs.up_vec.v[1];
sim->rates_bf[1] = - sim->ahrs.up_vec.v[0];
sim->rates_bf[2] = 0;
}
sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe);
sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up);
sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front);
for (i = 0; i < 3; i++)
{
qtmp1.v[i] = sim->vel[i];
}
qtmp1.s = 0.0f;
qvel_bf = quaternions_global_to_local(sim->ahrs.qe, qtmp1);
float acc_bf[3];
for (i = 0; i < 3; i++)
{
sim->vel_bf[i] = qvel_bf.v[i];
// following the convention in the IMU, this is the acceleration due to force, as measured
sim->ahrs.linear_acc[i] = sim->lin_forces_bf[i] / sim->vehicle_config.total_mass;
// this is the "clean" acceleration without gravity
acc_bf[i] = sim->ahrs.linear_acc[i] - sim->ahrs.up_vec.v[i] * sim->vehicle_config.sim_gravity;
sim->vel_bf[i] = sim->vel_bf[i] + acc_bf[i] * sim->dt;
}
// calculate velocity in global frame
// vel = qe *vel_bf * qe - 1
qvel_bf.s = 0.0f; qvel_bf.v[0] = sim->vel_bf[0]; qvel_bf.v[1] = sim->vel_bf[1]; qvel_bf.v[2] = sim->vel_bf[2];
qtmp1 = quaternions_local_to_global(sim->ahrs.qe, qvel_bf);
sim->vel[0] = qtmp1.v[0]; sim->vel[1] = qtmp1.v[1]; sim->vel[2] = qtmp1.v[2];
for (i = 0; i < 3; i++)
{
sim->local_position.pos[i] = sim->local_position.pos[i] + sim->vel[i] * sim->dt;
}
// fill in simulated IMU values
for (i = 0;i < 3; i++)
{
sim->imu->raw_gyro.data[sim->imu->calib_gyro.axis[i]] = (sim->rates_bf[i] * sim->calib_gyro.scale_factor[i] + sim->calib_gyro.bias[i]) * sim->calib_gyro.orientation[i];
sim->imu->raw_accelero.data[sim->imu->calib_accelero.axis[i]] = ((sim->lin_forces_bf[i] / sim->vehicle_config.total_mass / sim->vehicle_config.sim_gravity) * sim->calib_accelero.scale_factor[i] + sim->calib_accelero.bias[i]) * sim->calib_accelero.orientation[i];
sim->imu->raw_compass.data[sim->imu->calib_compass.axis[i]] = ((sim->ahrs.north_vec.v[i] ) * sim->calib_compass.scale_factor[i] + sim->calib_compass.bias[i])* sim->calib_compass.orientation[i];
}
sim->local_position.heading = coord_conventions_get_yaw(sim->ahrs.qe);
}
void simulation_simulate_barometer(simulation_model_t *sim)
{
sim->pressure->altitude = sim->local_position.origin.altitude - sim->local_position.pos[Z];
sim->pressure->vario_vz = sim->vel[Z];
sim->pressure->last_update = time_keeper_get_millis();
sim->pressure->altitude_offset = 0;
}
void simulation_simulate_gps(simulation_model_t *sim)
{
global_position_t gpos = coord_conventions_local_to_global_position(sim->local_position);
sim->gps->altitude = gpos.altitude;
sim->gps->latitude = gpos.latitude;
sim->gps->longitude = gpos.longitude;
sim->gps->time_last_msg = time_keeper_get_millis();
sim->gps->time_last_posllh_msg = time_keeper_get_millis();
sim->gps->time_last_velned_msg = time_keeper_get_millis();
sim->gps->north_speed = sim->vel[X];
sim->gps->east_speed = sim->vel[Y];
sim->gps->vertical_speed = sim->vel[Z];
sim->gps->status = GPS_OK;
sim->gps->healthy = true;
}
static void position_estimation_position_correction(position_estimator_t *pos_est)
{
global_position_t global_gps_position;
local_coordinates_t local_coordinates;
float dt = pos_est->ahrs->dt;
// quat_t bias_correction = {.s = 0, .v = {0.0f, 0.0f, 1.0f}};
quat_t vel_correction =
{
.s = 0,
.v =
{
0.0f,
0.0f,
1.0f
}
};
float pos_error[3] =
{
0.0f,
0.0f,
0.0f
};
float baro_alt_error = 0.0f;
float baro_vel_error = 0.0f;
float baro_gain = 0.0f;
float gps_gain = 0.0f;
float gps_dt = 0.0f;
float vel_error[3] =
{
0.0f,
0.0f,
0.0f
};
uint32_t t_inter_gps, t_inter_baro;
int32_t i;
if (pos_est->init_barometer)
{
// altimeter correction
if ( pos_est->time_last_barometer_msg < pos_est->barometer->last_update )
{
pos_est->last_alt = -(pos_est->barometer->altitude ) + pos_est->local_position.origin.altitude;
pos_est->time_last_barometer_msg = pos_est->barometer->last_update;
}
t_inter_baro = (time_keeper_get_micros() - pos_est->barometer->last_update) / 1000.0f;
baro_gain = 1.0f; //math_util_fmax(1.0f - t_inter_baro / 1000.0f, 0.0f);
//pos_est->local_position.pos[2] += kp_alt_baro / ((float)(t_inter_baro / 2.5f + 1.0f)) * alt_error;
baro_alt_error = pos_est->last_alt - pos_est->local_position.pos[2];
baro_vel_error = pos_est->barometer->vario_vz - pos_est->vel[2];
//vel_error[2] = 0.1f * pos_error[2];
//pos_est->vel[2] += kp_alt_baro_v * vel_error[2];
}
else
{
bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude);
pos_est->init_barometer = true;
}
if (pos_est->init_gps_position)
{
if ( (pos_est->time_last_gps_msg < pos_est->gps->time_last_msg) && (pos_est->gps->status == GPS_OK) )
{
pos_est->time_last_gps_msg = pos_est->gps->time_last_msg;
global_gps_position.longitude = pos_est->gps->longitude;
global_gps_position.latitude = pos_est->gps->latitude;
global_gps_position.altitude = pos_est->gps->altitude;
global_gps_position.heading = 0.0f;
local_coordinates = coord_conventions_global_to_local_position(global_gps_position,pos_est->local_position.origin);
local_coordinates.timestamp_ms = pos_est->gps->time_last_msg;
// compute GPS velocity estimate
gps_dt = (local_coordinates.timestamp_ms - pos_est->last_gps_pos.timestamp_ms) / 1000.0f;
if (gps_dt > 0.001f)
{
for (i = 0; i < 3; i++)
{
pos_est->last_vel[i] = (local_coordinates.pos[i] - pos_est->last_gps_pos.pos[i]) / gps_dt;
}
pos_est->last_gps_pos = local_coordinates;
}
else
{
print_util_dbg_print("GPS dt is too small!\r\n");
}
}
t_inter_gps = time_keeper_get_millis() - pos_est->gps->time_last_msg;
//gps_gain = math_util_fmax(1.0f - t_inter_gps / 1000.0f, 0.0f);
gps_gain = 1.0f;
for (i = 0;i < 3;i++)
{
pos_error[i] = pos_est->last_gps_pos.pos[i] - pos_est->local_position.pos[i];
vel_error[i] = pos_est->last_vel[i] - pos_est->vel[i];
}
}
else
{
gps_position_init(pos_est);
for (i = 0;i < 2;i++)
{
pos_error[i] = 0.0f;
vel_error[i] = 0.0f;
}
gps_gain = 0.1f;
}
// Apply error correction to velocity and position estimates
for (i = 0;i < 3;i++)
{
pos_est->local_position.pos[i] += pos_est->kp_pos_gps[i] * gps_gain * pos_error[i]* dt;
}
pos_est->local_position.pos[2] += pos_est->kp_alt_baro * baro_gain * baro_alt_error* dt;
for (i = 0; i < 3; i++)
{
vel_correction.v[i] = vel_error[i];
}
for (i = 0;i < 3;i++)
{
pos_est->vel[i] += pos_est->kp_vel_gps[i] * gps_gain * vel_correction.v[i]* dt;
}
pos_est->vel[2] += pos_est->kp_vel_baro * baro_gain * baro_vel_error* dt;
}
static void gps_position_init(position_estimator_t *pos_est)
{
int32_t i;
if ( (pos_est->init_gps_position == false)&&(pos_est->gps->status == GPS_OK) )
{
if ( pos_est->time_last_gps_msg < pos_est->gps->time_last_msg )
{
pos_est->time_last_gps_msg = pos_est->gps->time_last_msg;
pos_est->init_gps_position = true;
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;
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;
}
print_util_dbg_print("GPS position initialized!\r\n");
}
}
}
static void position_estimation_position_correction(position_estimation_t *pos_est)
{
global_position_t global_gps_position;
local_coordinates_t local_coordinates;
float dt = pos_est->ahrs->dt;
// quat_t bias_correction = {.s = 0, .v = {0.0f, 0.0f, 1.0f}};
quat_t vel_correction =
{
.s = 0,
.v =
{
0.0f,
0.0f,
1.0f
}
};
float pos_error[3] =
{
0.0f,
0.0f,
0.0f
};
float baro_alt_error = 0.0f;
float baro_vel_error = 0.0f;
float baro_gain = 0.0f;
float gps_gain = 0.0f;
float gps_dt = 0.0f;
float vel_error[3] =
{
0.0f,
0.0f,
0.0f
};
uint32_t t_inter_gps, t_inter_baro;
int32_t i;
if (pos_est->init_barometer)
{
// altimeter correction
if ( pos_est->time_last_barometer_msg < pos_est->barometer->last_update )
{
pos_est->last_alt = -(pos_est->barometer->altitude ) + pos_est->local_position.origin.altitude;
baro_alt_error = -(pos_est->barometer->altitude ) - pos_est->local_position.pos[2] + pos_est->local_position.origin.altitude;
pos_est->time_last_barometer_msg = pos_est->barometer->last_update;
}
t_inter_baro = (time_keeper_get_micros() - pos_est->barometer->last_update) / 1000.0f;
baro_gain = 1.0f; //math_util_fmax(1.0f - t_inter_baro / 1000.0f, 0.0f);
//pos_est->local_position.pos[2] += kp_alt_baro / ((float)(t_inter_baro / 2.5f + 1.0f)) * alt_error;
baro_alt_error = pos_est->last_alt - pos_est->local_position.pos[2];
baro_vel_error = pos_est->barometer->vario_vz - pos_est->vel[2];
//vel_error[2] = 0.1f * pos_error[2];
//pos_est->vel[2] += kp_alt_baro_v * vel_error[2];
}
else
{
bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude);
pos_est->init_barometer = true;
}
if (pos_est->init_gps_position)
{
if ( (pos_est->time_last_gps_msg < pos_est->gps->time_last_msg) && (pos_est->gps->status == GPS_OK) )
{
pos_est->time_last_gps_msg = pos_est->gps->time_last_msg;
global_gps_position.longitude = pos_est->gps->longitude;
global_gps_position.latitude = pos_est->gps->latitude;
global_gps_position.altitude = pos_est->gps->altitude;
global_gps_position.heading = 0.0f;
local_coordinates = coord_conventions_global_to_local_position(global_gps_position,pos_est->local_position.origin);
local_coordinates.timestamp_ms = pos_est->gps->time_last_msg;
// compute GPS velocity estimate
gps_dt = (local_coordinates.timestamp_ms - pos_est->last_gps_pos.timestamp_ms) / 1000.0f;
if (gps_dt > 0.001f)
{
for (i = 0; i < 3; i++)
{
pos_est->last_vel[i] = (local_coordinates.pos[i] - pos_est->last_gps_pos.pos[i]) / gps_dt;
}
pos_est->last_gps_pos = local_coordinates;
}
else
{
print_util_dbg_print("GPS dt is too small!\r\n");
}
}
t_inter_gps = time_keeper_get_millis() - pos_est->gps->time_last_msg;
//gps_gain = math_util_fmax(1.0f - t_inter_gps / 1000.0f, 0.0f);
gps_gain = 1.0f;
for (i = 0;i < 3;i++)
{
pos_error[i] = pos_est->last_gps_pos.pos[i] - pos_est->local_position.pos[i];
vel_error[i] = pos_est->last_vel[i] - pos_est->vel[i];
}
}
else
{
gps_position_init(pos_est);
for (i = 0;i < 2;i++)
{
pos_error[i] = 0.0f;
vel_error[i] = 0.0f;
}
gps_gain = 0.1f;
}
// Apply error correction to velocity and position estimates
for (i = 0;i < 3;i++)
{
pos_est->local_position.pos[i] += pos_est->kp_pos_gps[i] * gps_gain * pos_error[i]* dt;
}
pos_est->local_position.pos[2] += pos_est->kp_alt_baro * baro_gain * baro_alt_error* dt;
for (i = 0; i < 3; i++)
{
vel_correction.v[i] = vel_error[i];
}
for (i = 0;i < 3;i++)
{
pos_est->vel[i] += pos_est->kp_vel_gps[i] * gps_gain * vel_correction.v[i]* dt;
}
pos_est->vel[2] += pos_est->kp_vel_baro * baro_gain * baro_vel_error* dt;
}
static void gps_position_init(position_estimation_t *pos_est)
{
if ( (pos_est->init_gps_position == false)&&(pos_est->gps->status == GPS_OK) )
{
if ( pos_est->time_last_gps_msg < pos_est->gps->time_last_msg )
{
pos_est->time_last_gps_msg = pos_est->gps->time_last_msg;
pos_est->init_gps_position = true;
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;
pos_est->last_alt = 0;
for(int32_t 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;
}
print_util_dbg_print("GPS position initialized!\r\n");
}
}
}
//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------
void position_estimation_init(position_estimation_t* pos_est, const position_estimation_conf_t* config, state_t* state, barometer_t *barometer, const gps_t *gps, const ahrs_t *ahrs, const imu_t *imu)
{
//init dependencies
pos_est->barometer = barometer;
pos_est->gps = gps;
pos_est->ahrs = ahrs;
pos_est->imu = imu;
pos_est->state = state;
pos_est->nav_plan_active = &state->nav_plan_active;
// default GPS home position
pos_est->local_position.origin.longitude = config->origin.longitude;
pos_est->local_position.origin.latitude = config->origin.latitude;
pos_est->local_position.origin.altitude = config->origin.altitude;
pos_est->local_position.pos[X] = 0;
pos_est->local_position.pos[Y] = 0;
pos_est->local_position.pos[Z] = 0;
// reset position estimator
pos_est->last_alt = 0;
for(int32_t i = 0;i < 3;i++)
{
pos_est->last_vel[i] = 0.0f;
pos_est->vel[i] = 0.0f;
pos_est->vel_bf[i] = 0.0f;
}
pos_est->gravity = config->gravity;
pos_est->init_gps_position = false;
pos_est->init_barometer = false;
pos_est->time_last_gps_msg = 0;
pos_est->time_last_barometer_msg = 0;
pos_est->kp_pos_gps[X] = 2.0f;
pos_est->kp_pos_gps[Y] = 2.0f;
pos_est->kp_pos_gps[Z] = 0.0f;
pos_est->kp_vel_gps[X] = 1.0f;
pos_est->kp_vel_gps[Y] = 1.0f;
pos_est->kp_vel_gps[Z] = 0.5f;
pos_est->kp_alt_baro = 2.0f;
pos_est->kp_vel_baro = 1.0f;
gps_position_init(pos_est);
print_util_dbg_print("Position estimation initialized.\r\n");
}
