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; }