Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updates to Mavlink for zero based WP counting

This commit is contained in:
Jason Short 2011-11-12 21:44:38 -08:00
parent e1315b1ab8
commit 901e46334d
1 changed files with 30 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
ArduCopter/GCS_Mavlink.pde
View File

@ -66,6 +66,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
break; break;
case GUIDED: case GUIDED:
mode = MAV_MODE_GUIDED; mode = MAV_MODE_GUIDED;
nav_mode = MAV_NAV_WAYPOINT;
break; break;
default: default:
mode = control_mode + 100; mode = control_mode + 100;
@ -579,7 +580,7 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis(); uint32_t tnow = millis();
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total && waypoint_request_i < (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500) { tnow > waypoint_timelast_request + 500) {
waypoint_timelast_request = tnow; waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
@ -947,7 +948,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_count_send( mavlink_msg_waypoint_count_send(
chan,msg->sysid, chan,msg->sysid,
msg->compid, msg->compid,
g.command_total + 1); // + home g.command_total); // includes home
waypoint_timelast_send = millis(); waypoint_timelast_send = millis();
waypoint_sending = true; waypoint_sending = true;
@ -1005,8 +1006,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
} }
// Switch to map APM command fields inot MAVLink command fields
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields switch (tell_command.id) {
case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_CONDITION_CHANGE_ALT:
@ -1122,7 +1123,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// clear all waypoints // clear all waypoints
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
g.command_total.set_and_save(0); g.command_total.set_and_save(1);
// send acknowledgement 3 times to makes sure it is received // send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
@ -1160,7 +1161,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.count > MAX_WAYPOINTS) { if (packet.count > MAX_WAYPOINTS) {
packet.count = MAX_WAYPOINTS; packet.count = MAX_WAYPOINTS;
} }
g.command_total.set_and_save(packet.count - 1); g.command_total.set_and_save(packet.count);
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_receiving = true; waypoint_receiving = true;
@ -1313,28 +1314,33 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Check if receiving waypoints (mission upload expected) // Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) break; if (!waypoint_receiving) break;
Serial.printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
// check if this is the requested waypoint // check if this is the requested waypoint
if (packet.seq != waypoint_request_i) break; if (packet.seq != waypoint_request_i)
set_command_with_index(tell_command, packet.seq); break;
// update waypoint receiving state machine set_command_with_index(tell_command, packet.seq);
waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.command_total){ // update waypoint receiving state machine
uint8_t type = 0; // ok (0), error(1) waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_request_i++;
mavlink_msg_waypoint_ack_send( if (waypoint_request_i == (uint16_t)g.command_total){
chan, uint8_t type = 0; // ok (0), error(1)
msg->sysid,
msg->compid,
type);
send_text(SEVERITY_LOW,PSTR("flight plan received")); mavlink_msg_waypoint_ack_send(
waypoint_receiving = false; chan,
// XXX ignores waypoint radius for individual waypoints, can msg->sysid,
// only set WP_RADIUS parameter msg->compid,
type);
send_text(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} }
} }
break; break;
@ -1657,7 +1663,7 @@ void
GCS_MAVLINK::queued_waypoint_send() GCS_MAVLINK::queued_waypoint_send()
{ {
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total) { waypoint_request_i < (unsigned)g.command_total) {
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,