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