aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mavlink: merged in changes from APM

Browse Source 
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 2011-03-19 08:05:06 +00:00
parent 9baf649923
commit 0374781c56
1 changed files with 45 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
ArduCopterMega/GCS_Mavlink.pde
View File

@ -53,13 +53,13 @@ GCS_MAVLINK::update(void)
// stop waypoint sending if timeout // stop waypoint sending if timeout
if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_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; global_data.waypoint_sending = false;
} }
// stop waypoint receiving if timeout // stop waypoint receiving if timeout
if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_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; 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; if (mavlink_check_target(packet.target,packet.target_component)) break;
// do action // do action
send_text(SEVERITY_LOW,"action received"); send_text(SEVERITY_LOW,PSTR("action received"));
switch(packet.action){ switch(packet.action){
case MAV_ACTION_LAUNCH: case MAV_ACTION_LAUNCH:
@ -286,7 +286,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{ {
//send_text(SEVERITY_LOW,"waypoint request list"); //send_text(SEVERITY_LOW,PSTR("waypoint request list"));
// decode // decode
mavlink_waypoint_request_list_t packet; mavlink_waypoint_request_list_t packet;
@ -312,7 +312,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{ {
//send_text(SEVERITY_LOW,"waypoint request"); //send_text(SEVERITY_LOW,PSTR("waypoint request"));
// Check if sending waypiont // Check if sending waypiont
if (!global_data.waypoint_sending) break; if (!global_data.waypoint_sending) break;
@ -392,7 +392,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_WAYPOINT_ACK: case MAVLINK_MSG_ID_WAYPOINT_ACK:
{ {
//send_text(SEVERITY_LOW,"waypoint ack"); //send_text(SEVERITY_LOW,PSTR("waypoint ack"));
// decode // decode
mavlink_waypoint_ack_t packet; mavlink_waypoint_ack_t packet;
@ -409,7 +409,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{ {
//send_text(SEVERITY_LOW,"param request list"); //send_text(SEVERITY_LOW,PSTR("param request list"));
// decode // decode
mavlink_param_request_list_t packet; 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: case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{ {
//send_text(SEVERITY_LOW,"waypoint clear all"); //send_text(SEVERITY_LOW,PSTR("waypoint clear all"));
// decode // decode
mavlink_waypoint_clear_all_t packet; 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: case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{ {
//send_text(SEVERITY_LOW,"waypoint set current"); //send_text(SEVERITY_LOW,PSTR("waypoint set current"));
// decode // decode
mavlink_waypoint_set_current_t packet; mavlink_waypoint_set_current_t packet;
@ -466,7 +466,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_WAYPOINT_COUNT: case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{ {
//send_text(SEVERITY_LOW,"waypoint count"); //send_text(SEVERITY_LOW,PSTR("waypoint count"));
// decode // decode
mavlink_waypoint_count_t packet; mavlink_waypoint_count_t packet;
@ -587,7 +587,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
msg->compid, msg->compid,
type); type);
send_text(SEVERITY_LOW,"flight plan received"); send_text(SEVERITY_LOW,PSTR("flight plan received"));
global_data.waypoint_receiving = false; global_data.waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can // XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter // only set WP_RADIUS parameter
@ -618,34 +618,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
!isnan(packet.param_value) && // not nan !isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf !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 // fetch the variable type ID
var_type = vp->meta_type_id(); var_type = vp->meta_type_id();
// handle variables with standard type IDs // handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) { 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) { } 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) { } 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) { } 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) { } 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 // Report back the new value if we accepted the change
mavlink_msg_param_value_send( // we send the value we actually set, which could be
chan, // different from the value sent, in case someone sent
(int8_t *)key, // a fractional value to an integer type
packet.param_value, mavlink_msg_param_value_send(
_count_parameters(), chan,
-1); // XXX we don't actually know what its index is... (int8_t *)key,
vp->cast_to_float(),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
}
break; break;
} // end case } // 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 // this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++; mavdelay++;
// request waypoints one by one // request waypoints one by one
// XXX note that this is pan-interface // XXX note that this is pan-interface
if (global_data.waypoint_receiving && if (global_data.waypoint_receiving &&
(global_data.requested_interface == chan) && (global_data.requested_interface == chan) &&
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)){ // limits to 3.33 hz global_data.waypoint_request_i <= g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
global_data.waypoint_dest_sysid, global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid, global_data.waypoint_dest_compid,
global_data.waypoint_request_i); global_data.waypoint_request_i);
mavdelay = 0; mavdelay = 0;
} }
} }
#endif #endif