bool state_telemetry_init(state_t* state, mavlink_message_handler_t *message_handler)
{
bool init_success = true;
// Add callbacks for onboard parameters requests
mavlink_message_handler_msg_callback_t callback;
callback.message_id = MAVLINK_MSG_ID_SET_MODE; // 11
callback.sysid_filter = MAVLINK_BASE_STATION_ID;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &state_telemetry_set_mav_mode;
callback.module_struct = (handling_module_struct_t) state;
init_success &= mavlink_message_handler_add_msg_callback( message_handler, &callback );
// Add callbacks for waypoint handler commands requests
mavlink_message_handler_cmd_callback_t callbackcmd;
callbackcmd.command_id = MAV_CMD_DO_SET_MODE; // 176
callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
callbackcmd.compid_filter = MAV_COMP_ID_ALL;
callbackcmd.compid_target = MAV_COMP_ID_ALL; // 0
callbackcmd.function = (mavlink_cmd_callback_function_t) &state_telemetry_set_mode_from_cmd;
callbackcmd.module_struct = state;
init_success &= mavlink_message_handler_add_cmd_callback(message_handler, &callbackcmd);
callbackcmd.command_id = MAV_CMD_DO_PARACHUTE; // 208
callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
callbackcmd.compid_filter = MAV_COMP_ID_ALL;
callbackcmd.compid_target = MAV_COMP_ID_SYSTEM_CONTROL; // 250
callbackcmd.function = (mavlink_cmd_callback_function_t) &state_telemetry_toggle_remote_use;
callbackcmd.module_struct = state;
init_success &= mavlink_message_handler_add_cmd_callback(message_handler, &callbackcmd);
return init_success;
}
bool joystick_parsing_telemetry_init(joystick_parsing_t* joystick_parsing, mavlink_message_handler_t* message_handler)
{
bool init_success = true;
// Add callbacks for waypoint handler messages requests
mavlink_message_handler_msg_callback_t callback;
callback.message_id = MAVLINK_MSG_ID_MANUAL_CONTROL; // 69
callback.sysid_filter = MAVLINK_BASE_STATION_ID;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &joystick_parsing_telemetry_parse_msg;
callback.module_struct = (handling_module_struct_t) joystick_parsing;
init_success &= mavlink_message_handler_add_msg_callback( message_handler, &callback );
return init_success;
}
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");
}
bool onboard_parameters_init(onboard_parameters_t* onboard_parameters, const onboard_parameters_conf_t* config, scheduler_t* scheduler, mavlink_message_handler_t* message_handler, const mavlink_stream_t* mavlink_stream)
{
bool init_success = true;
// Init dependencies
onboard_parameters->mavlink_stream = mavlink_stream;
// Init debug mode
onboard_parameters->debug = config->debug;
// Allocate memory for the onboard parameters
onboard_parameters->param_set = malloc( sizeof(onboard_parameters_set_t) + sizeof(onboard_parameters_entry_t[config->max_param_count]) );
if ( onboard_parameters->param_set != NULL )
{
onboard_parameters->param_set->max_param_count = config->max_param_count;
onboard_parameters->param_set->param_count = 0;
init_success &= true;
}
else
{
print_util_dbg_print("[ONBOARD PARAMETERS] ERROR ! Bad memory allocation\r\n");
onboard_parameters->param_set->max_param_count = 0;
onboard_parameters->param_set->param_count = 0;
init_success &= false;
}
// Add onboard parameter telemetry to the scheduler
init_success &= scheduler_add_task( scheduler,
100000,
RUN_REGULAR,
PERIODIC_ABSOLUTE,
PRIORITY_NORMAL,
(task_function_t)&onboard_parameters_send_scheduled_parameters,
(task_argument_t)onboard_parameters,
MAVLINK_MSG_ID_PARAM_VALUE);
// Add callbacks for onboard parameters requests
mavlink_message_handler_msg_callback_t callback;
callback.message_id = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; // 21
callback.sysid_filter = MAVLINK_BASE_STATION_ID;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &onboard_parameters_send_all_parameters;
callback.module_struct = (handling_module_struct_t) onboard_parameters;
init_success &= mavlink_message_handler_add_msg_callback( message_handler, &callback );
callback.message_id = MAVLINK_MSG_ID_PARAM_REQUEST_READ; // 20
callback.sysid_filter = MAVLINK_BASE_STATION_ID;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &onboard_parameters_send_parameter;
callback.module_struct = (handling_module_struct_t) onboard_parameters;
init_success &= mavlink_message_handler_add_msg_callback( message_handler, &callback );
callback.message_id = MAVLINK_MSG_ID_PARAM_SET; // 23
callback.sysid_filter = MAVLINK_BASE_STATION_ID;
callback.compid_filter = MAV_COMP_ID_ALL;
callback.function = (mavlink_msg_callback_function_t) &onboard_parameters_receive_parameter;
callback.module_struct = (handling_module_struct_t) onboard_parameters;
init_success &= mavlink_message_handler_add_msg_callback( message_handler, &callback );
// Add callbacks for waypoint handler commands requests
mavlink_message_handler_cmd_callback_t callbackcmd;
callbackcmd.command_id = MAV_CMD_PREFLIGHT_STORAGE; // 245
callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
callbackcmd.compid_filter = MAV_COMP_ID_ALL;
callbackcmd.compid_target = MAV_COMP_ID_ALL;
callbackcmd.function = (mavlink_cmd_callback_function_t) &onboard_parameters_preflight_storage;
callbackcmd.module_struct = onboard_parameters;
init_success &= mavlink_message_handler_add_cmd_callback( message_handler, &callbackcmd);
print_util_dbg_print("[ONBOARD PARAMETERS] Initialised.\r\n");
return init_success;
}
