--- aq_mavlink.c (revision 449)
+++ aq_mavlink.c (working copy)
@@ -200,7 +200,7 @@
// position -- gps and ukf
if (streamAll || (mavlinkData.streams[MAV_DATA_STREAM_POSITION].enable && mavlinkData.streams[MAV_DATA_STREAM_POSITION].next < micros)) {
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, micros, navData.fixType, gpsData.lat*(double)1e7, gpsData.lon*(double)1e7, gpsData.height*1e3,
- gpsData.hAcc*100, gpsData.vAcc*100, gpsData.speed*100, gpsData.heading, 255);
+ gpsData.hAcc*100, gpsData.vAcc*100, gpsData.speed*100, gpsData.heading, gpsData.numSV);
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, micros, UKF_POSN, UKF_POSE, -UKF_POSD, UKF_VELN, UKF_VELE, -VELOCITYD);
mavlinkData.streams[MAV_DATA_STREAM_POSITION].next = micros + mavlinkData.streams[MAV_DATA_STREAM_POSITION].interval;
}
@@ -636,19 +636,43 @@
uint16_t seqId;
uint8_t ack = 0;
uint8_t frame;
+ uint8_t command;
+ uint8_t current;
+ command = mavlink_msg_mission_item_get_command(&msg);
+ current = mavlink_msg_mission_item_get_current(&msg);
seqId = mavlink_msg_mission_item_get_seq(&msg);
frame = mavlink_msg_mission_item_get_frame(&msg);
- if (seqId >= NAV_MAX_MISSION_LEGS || (frame != MAV_FRAME_GLOBAL && frame != MAV_FRAME_GLOBAL_RELATIVE_ALT)) {
+ if (seqId == 0 && command == MAV_CMD_NAV_WAYPOINT && current == 2) {
+ double targetLat = mavlink_msg_mission_item_get_x(&msg);
+ double targetLon = mavlink_msg_mission_item_get_y(&msg);
+ double targetAlt = mavlink_msg_mission_item_get_z(&msg);
+ if (((targetLat < (double)90.0) && (targetLat > (double)-90.0)) && ((targetLon < (double)180.0) && (targetLon > (double)-180.0))) {
+ if ( navData.mode == NAV_STATUS_POSHOLD ) {
+ navUkfSetGlobalPositionTarget((double)targetLat, (double)targetLon);
+ if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+ navSetHoldAlt(targetAlt, 1);
+ } else {
+ navSetHoldAlt(targetAlt, 0);
+ }
+ } else {
+ // NACK
+ ack = 1;
+ AQ_NOTICE("Error: not in poshold");
+ }
+ } else {
+ // NACK
+ ack = 1;
+ AQ_NOTICE("Error: position out of range");
+ }
+ }
+ else if (seqId >= NAV_MAX_MISSION_LEGS || (frame != MAV_FRAME_GLOBAL && frame != MAV_FRAME_GLOBAL_RELATIVE_ALT)) {
// NACK
ack = 1;
}
else {
navMission_t *wp;
- uint8_t command;
-
- command = mavlink_msg_mission_item_get_command(&msg);
if (command == MAV_CMD_NAV_RETURN_TO_LAUNCH) {
wp = navGetWaypoint(seqId);
@@ -683,7 +707,7 @@
}
else if (command == MAV_CMD_DO_SET_HOME) {
// use current location
- if (mavlink_msg_mission_item_get_current(&msg)) {
+ if (current) {
navSetHomeCurrent();
}
// use given location
