void Mission_handler_navigating::send_nav_time(const Mavlink_stream* mavlink_stream_, mavlink_message_t* msg) { mavlink_msg_named_value_int_pack(mavlink_stream_->sysid(), mavlink_stream_->compid(), msg, time_keeper_get_ms(), "travel_time_", travel_time_); }
void analog_monitor_telemetry_send_sonar(const analog_monitor_t* analog_monitor, const Mavlink_stream* mavlink_stream, mavlink_message_t* msg) { mavlink_msg_named_value_float_pack(mavlink_stream->sysid(), mavlink_stream->compid(), msg, time_keeper_get_ms(), "sonar", 1000.0f / 9.8f * 2.54f * analog_monitor->avg[ANALOG_RAIL_12]); }
void stabilisation_telemetry_send_rpy_rates_error(const stabiliser_t* stabiliser, const Mavlink_stream* 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_ms(), 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* 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_ms(), controls->rpy[ROLL], controls->rpy[PITCH], controls->rpy[YAW], controls->thrust); }
bool Data_logging::create_new_log_file(const char* file_name__, bool continuous_write__, uint32_t sysid) { bool init_success = true; // Allocate memory for the onboard data_log //data_logging_set = (data_logging_set_t*)malloc(sizeof(data_logging_set_t) + sizeof(data_logging_entry_t[config->max_data_logging_count])); file_init_ = false; file_opened_ = false; sys_status_ = true; // Setting the maximal size of the name string #if _USE_LFN buffer_name_size_ = _MAX_LFN; #else #ifdef _MAX_LFN buffer_name_size_ = 8; #else buffer_name_size_ = 255; #endif #endif // Allocating memory for the file name string file_name_ = (char*)malloc(buffer_name_size_); name_n_extension_ = (char*)malloc(buffer_name_size_); if (file_name_ == NULL) { init_success &= false; } if (name_n_extension_ == NULL) { init_success &= false; } sys_id_ = sysid; // Append sysid to filename filename_append_int(file_name_, (char*)file_name__, sysid, buffer_name_size_); init_success &= open_new_log_file(); logging_time_ = time_keeper_get_ms(); cksum_a_ = 0.0; cksum_b_ = 0.0; return init_success; }
bool Mission_handler_navigating::setup(const Waypoint& wpt) { bool success = true; start_time_ = time_keeper_get_ms(); waypoint_reached_ = false; waypoint_ = wpt; // Compute desired command local_position_t local_pos = ins_.position_lf(); position_command_.xyz = waypoint_.local_pos(); position_command_.heading = atan2(position_command_.xyz[Y] - local_pos[Y], position_command_.xyz[X] - local_pos[X]); return success; }
bool Data_logging::update(void) { uint32_t time_ms = 0; if (log_data_ == 1) { if (file_opened_) { if (!file_init_) { add_header_name(); } if (!state_.is_armed()) { time_ms = time_keeper_get_ms(); if ((time_ms - logging_time_) > 5000) { console_.get_stream()->flush(); logging_time_ = time_ms; } } if (continuous_write_) { log_parameters(); } else { if (checksum_control()) { log_parameters(); } } } //end of if (file_opened_) else { if (sys_status_) { open_new_log_file(); } cksum_a_ = 0.0; cksum_b_ = 0.0; }//end of else if (file_opened_) } //end of if (log_data_ == 1) else { sys_status_ = true; if (file_opened_) { bool succeed = console_.get_stream()->close(); cksum_a_ = 0.0; cksum_b_ = 0.0; file_opened_ = false; file_init_ = false; if (debug_) { if (succeed) { print_util_dbg_print("File closed\r\n"); } else { print_util_dbg_print("Error closing file\r\n"); } } } //end of if (file_opened_) } //end of else (log_data_ != 1) return true; }
void Data_logging::log_parameters(void) { uint32_t i; bool success = true; // First parameter is always time uint32_t time_ms = time_keeper_get_ms(); success &= console_.write(time_ms); put_r_or_n(0); for (i = 0; i < data_logging_count_; i++) { // Writing the value of the parameter to the file, separate values by tab character data_logging_entry_t* param = &data_log_[i]; switch (param->data_type) { case MAV_PARAM_TYPE_UINT8: success &= console_.write(*((uint8_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_INT8: success &= console_.write(*((int8_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_UINT16: success &= console_.write(*((uint16_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_INT16: success &= console_.write(*((int16_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_UINT32: success &= console_.write(*((uint32_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_INT32: success &= console_.write(*((int32_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_UINT64: success &= console_.write(*((uint64_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_INT64: success &= console_.write(*((int64_t*)param->param)); put_r_or_n(i); break; case MAV_PARAM_TYPE_REAL32: success &= console_.write(*(float*)param->param, param->precision); put_r_or_n(i); break; case MAV_PARAM_TYPE_REAL64: success &= console_.write(*((double*)param->param), param->precision); put_r_or_n(i); break; default: success &= false; print_util_dbg_print("Data type not supported!\r\n"); break; } if (!success) { if (debug_) { print_util_dbg_print("Error appending parameter! Error:"); } break; } } }
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; }