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