void onboard_parameters_receive_parameter(onboard_parameters_t* onboard_parameters, uint32_t sysid, mavlink_message_t* msg) { bool match = true; mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system and subsystem if ( ((uint8_t)set.target_system == (uint8_t)sysid ) && (set.target_component == onboard_parameters->mavlink_stream->compid) ) { char* key = (char*) set.param_id; onboard_parameters_entry_t* param; if ( onboard_parameters->debug == true ) { print_util_dbg_print("Setting parameter "); print_util_dbg_print(set.param_id); print_util_dbg_print(" to "); print_util_dbg_putfloat(set.param_value, 2); print_util_dbg_print("\r\n"); } for (uint16_t i = 0; i < onboard_parameters->param_set->param_count; i++) { param = &onboard_parameters->param_set->parameters[i]; match = true; for (uint16_t j = 0; j < param->param_name_length; j++) { // Compare if ((char)param->param_name[j] != (char)key[j]) { match = false; } // End matching if null termination is reached if (((char)param->param_name[j]) == '\0') { // Exit internal (j) for() loop break; } } // Check if matched if (match) { // Only write and emit changes if there is actually a difference if (*(param->param) != set.param_value && set.param_type == param->data_type) { // onboard_parameters_update_parameter(onboard_parameters, i, set.param_value); (*param->param) = set.param_value; // schedule parameter for transmission downstream param->schedule_for_transmission=true; } break; } } if (!match) { if ( onboard_parameters->debug == true ) { print_util_dbg_print("Set parameter error! Parameter "); print_util_dbg_print(set.param_id); print_util_dbg_print(" not registred!\r\n"); } } } }
Mission_handler::update_status_t Mission_handler_navigating::update() { /********************************** Determine if arrived for first time **********************************/ // Find distance to waypoint local_position_t wpt_pos = waypoint_.local_pos(); float rel_pos[3]; for (int i = 0; i < 3; i++) { rel_pos[i] = wpt_pos[i] - ins_.position_lf()[i]; } float dist2wp_sqr = vectors_norm_sqr(rel_pos); float vel_sqr = vectors_norm_sqr(ins_.velocity_lf().data()); // Check if radius is available float radius = 0.0f; waypoint_.radius(radius); if (radius == 0.0f) { // TODO use a configuration for this default radius radius = 2.0f; } // Check if we reached the waypoint bool is_arrived = (dist2wp_sqr < (radius * radius)) && (vel_sqr < 1.0f); // TODO use a config for this speed threshold if (is_arrived) { // If we are near the waypoint but the flag has not been set, do this once ... if (!waypoint_reached_) { // Send debug log ... print_util_dbg_print("Waypoint reached, distance: "); print_util_dbg_putfloat(sqrt(dist2wp_sqr), 3); print_util_dbg_print(" m. Less than acceptable radius:"); print_util_dbg_putfloat(sqrt(radius * radius), 3); print_util_dbg_print(" m.\r\n"); // ... as well as a mavlink message ... mavlink_message_t msg; mavlink_msg_mission_item_reached_pack(mavlink_stream_.sysid(), mavlink_stream_.compid(), &msg, waypoint_handler_.current_waypoint_index()); mavlink_stream_.send(&msg); // ... and record the travel time ... travel_time_ = time_keeper_get_ms() - start_time_; // ... and set to waiting at waypoint ... waypoint_reached_ = true; // If the waypoint is not autocontinue, use its heading if (waypoint_.autocontinue() == false) { // Use waypoint's heading waypoint_.heading(position_command_.heading); } } } /******************* Determine status code ********************/ // First check if we have reached the waypoint if (waypoint_reached_) { // Then ensure that we are in autocontinue if ((waypoint_.autocontinue() == 1) && (waypoint_handler_.waypoint_count() > 1)) { // Differentiate between dubin and direct to return MISSION_FINISHED; } } return MISSION_IN_PROGRESS; }