mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
mavlink: merged in changes from APM
this fixes waypoint upload git-svn-id: https://arducopter.googlecode.com/svn/trunk@1787 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com
parent
2427931550
commit
8ca4986375
@ -53,13 +53,13 @@ GCS_MAVLINK::update(void)
|
||||
|
||||
// stop waypoint sending if timeout
|
||||
if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){
|
||||
send_text(SEVERITY_LOW,"waypoint send timeout");
|
||||
send_text(SEVERITY_LOW,PSTR("waypoint send timeout"));
|
||||
global_data.waypoint_sending = false;
|
||||
}
|
||||
|
||||
// stop waypoint receiving if timeout
|
||||
if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){
|
||||
send_text(SEVERITY_LOW,"waypoint receive timeout");
|
||||
send_text(SEVERITY_LOW,PSTR("waypoint receive timeout"));
|
||||
global_data.waypoint_receiving = false;
|
||||
}
|
||||
}
|
||||
@ -193,7 +193,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
if (mavlink_check_target(packet.target,packet.target_component)) break;
|
||||
|
||||
// do action
|
||||
send_text(SEVERITY_LOW,"action received");
|
||||
send_text(SEVERITY_LOW,PSTR("action received"));
|
||||
switch(packet.action){
|
||||
|
||||
case MAV_ACTION_LAUNCH:
|
||||
@ -286,7 +286,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint request list");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint request list"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_request_list_t packet;
|
||||
@ -312,7 +312,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint request");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint request"));
|
||||
|
||||
// Check if sending waypiont
|
||||
if (!global_data.waypoint_sending) break;
|
||||
@ -392,7 +392,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint ack");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint ack"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_ack_t packet;
|
||||
@ -409,7 +409,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"param request list");
|
||||
//send_text(SEVERITY_LOW,PSTR("param request list"));
|
||||
|
||||
// decode
|
||||
mavlink_param_request_list_t packet;
|
||||
@ -425,7 +425,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint clear all");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint clear all"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_clear_all_t packet;
|
||||
@ -444,7 +444,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint set current");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint set current"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_set_current_t packet;
|
||||
@ -466,7 +466,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
|
||||
{
|
||||
//send_text(SEVERITY_LOW,"waypoint count");
|
||||
//send_text(SEVERITY_LOW,PSTR("waypoint count"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_count_t packet;
|
||||
@ -587,7 +587,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
msg->compid,
|
||||
type);
|
||||
|
||||
send_text(SEVERITY_LOW,"flight plan received");
|
||||
send_text(SEVERITY_LOW,PSTR("flight plan received"));
|
||||
global_data.waypoint_receiving = false;
|
||||
// XXX ignores waypoint radius for individual waypoints, can
|
||||
// only set WP_RADIUS parameter
|
||||
@ -618,34 +618,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
!isnan(packet.param_value) && // not nan
|
||||
!isinf(packet.param_value)) { // not inf
|
||||
|
||||
// add a small amount before casting parameter values
|
||||
// from float to integer to avoid truncating to the
|
||||
// next lower integer value.
|
||||
const float rounding_addition = 0.01;
|
||||
|
||||
// fetch the variable type ID
|
||||
var_type = vp->meta_type_id();
|
||||
|
||||
// handle variables with standard type IDs
|
||||
if (var_type == AP_Var::k_typeid_float) {
|
||||
((AP_Float *)vp)->set(packet.param_value);
|
||||
((AP_Float *)vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_float16) {
|
||||
((AP_Float16 *)vp)->set(packet.param_value);
|
||||
((AP_Float16 *)vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int32) {
|
||||
((AP_Int32 *)vp)->set(packet.param_value);
|
||||
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int16) {
|
||||
((AP_Int16 *)vp)->set(packet.param_value);
|
||||
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int8) {
|
||||
((AP_Int8 *)vp)->set(packet.param_value);
|
||||
((AP_Int8 *)vp)->set(packet.param_value+rounding_addition);
|
||||
} else {
|
||||
// we don't support mavlink set on this parameter
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Report back new value
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
(int8_t *)key,
|
||||
packet.param_value,
|
||||
_count_parameters(),
|
||||
-1); // XXX we don't actually know what its index is...
|
||||
// Report back the new value if we accepted the change
|
||||
// we send the value we actually set, which could be
|
||||
// different from the value sent, in case someone sent
|
||||
// a fractional value to an integer type
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
(int8_t *)key,
|
||||
vp->cast_to_float(),
|
||||
_count_parameters(),
|
||||
-1); // XXX we don't actually know what its index is...
|
||||
}
|
||||
|
||||
break;
|
||||
} // end case
|
||||
@ -845,22 +856,22 @@ GCS_MAVLINK::_queued_send()
|
||||
}
|
||||
|
||||
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
|
||||
mavdelay++;
|
||||
mavdelay++;
|
||||
|
||||
// request waypoints one by one
|
||||
// XXX note that this is pan-interface
|
||||
if (global_data.waypoint_receiving &&
|
||||
(global_data.requested_interface == chan) &&
|
||||
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)){ // limits to 3.33 hz
|
||||
|
||||
mavlink_msg_waypoint_request_send(
|
||||
chan,
|
||||
global_data.waypoint_dest_sysid,
|
||||
global_data.waypoint_dest_compid,
|
||||
if (global_data.waypoint_receiving &&
|
||||
(global_data.requested_interface == chan) &&
|
||||
global_data.waypoint_request_i <= g.waypoint_total &&
|
||||
mavdelay > 15) { // limits to 3.33 hz
|
||||
mavlink_msg_waypoint_request_send(
|
||||
chan,
|
||||
global_data.waypoint_dest_sysid,
|
||||
global_data.waypoint_dest_compid,
|
||||
global_data.waypoint_request_i);
|
||||
|
||||
mavdelay = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user