diff --git a/CHANGES.md b/CHANGES.md
index 8e3ce788..b874a9ab 100644
--- a/CHANGES.md
+++ b/CHANGES.md
@@ -10,6 +10,13 @@ This file describes changes in the firmware, typically since the previous minor
! : important change, possible danger, change of default behavior, etc.
+##### 7.1.1887 - Feb. 21, 2016
+
+'*' Fix random yawing on Landing-type waypoint when previous wpt type used relative heading.
+`*` Fix setting of desired yaw/heading for Landing waypoint via MAVLink/QGC.
+`~` Specifically hold current position for Landing waypoint (possibly prevent diagonal descent). (Note that Lat/Lon cannot be specified for landing-type waypoint, which has always been the case but is not obvious when using the QGC mission planner.)
+`+` Add more diagnostic telemetry data (RC switch statuses and RAM usage).
+
##### 7.1.1885 - Feb. 9, 2016
`!` Waypoint skip switch will now go back to start of mission after the last waypoint, allows restarting a mission in flight.
@@ -23,7 +30,7 @@ This file describes changes in the firmware, typically since the previous minor
`~` Enable 2nd serial port option on M4 boards (COMM 2). Shared with PWM ports 7 (Serial Tx, J2.10 on expansion header) and 8 (Serial Rx, J1.10 on header). When using those ports for PWM (motor) outputs, be sure to **disable any protocol on Serial 2** (QGC Misc. Settings/Serial Port 2/Protocol = None).
`*` Fix possible endless notification loop if sending of waypoints times out.
`-` Remove State Of Charge (SOC) parameters and functions since they're not used.
-`~` Start up message notification queue early to allow more startup messages to come through. Increase size of notification queue to avoid loosing startup messages before comm system is active.
+`~` Start up message notification queue early to allow more startup messages to come through. Increase size of notification queue to avoid loosing startup messages before comm system is active.
`~` Exclude unused AQ binary telemetry/command interface from default builds (define HAS_AQ_TELEMETRY to include it).
##### 7.1.1871 - Dec. 7, 2015
diff --git a/lib/mavlink b/lib/mavlink
index 67a140b3..3139e509 160000
--- a/lib/mavlink
+++ b/lib/mavlink
@@ -1 +1 @@
-Subproject commit 67a140b30b9878a94445854e624eb1cffd4cbd37
+Subproject commit 3139e5094abb684f485851ccce928d47525ad89d
diff --git a/src/aq_mavlink.c b/src/aq_mavlink.c
index 1c4fd0f7..601013ac 100644
--- a/src/aq_mavlink.c
+++ b/src/aq_mavlink.c
@@ -14,6 +14,7 @@
along with AutoQuad. If not, see .
Copyright © 2011-2014 Bill Nesbitt
+ Copyright 2013-2016 Maxim Paperno
*/
#include "aq.h"
@@ -91,12 +92,12 @@ void mavlinkSendNotice(const char *s) {
mavlink_msg_statustext_send(MAVLINK_COMM_0, MAV_SEVERITY_INFO, (const char *)s);
}
-void mavlinkToggleStreams(uint8_t enable) {
+static void mavlinkToggleStreams(uint8_t enable) {
for (uint8_t i = 0; i < AQMAVLINK_TOTAL_STREAMS; i++)
mavlinkData.streams[i].enable = enable && mavlinkData.streams[i].interval;
}
-void mavlinkSetSystemData(void) {
+static void mavlinkSetSystemData(void) {
if ((supervisorData.state & STATE_RADIO_LOSS1) || (supervisorData.state & STATE_LOW_BATTERY2))
mavlinkData.sys_state = MAV_STATE_CRITICAL;
@@ -140,7 +141,7 @@ void mavlinkDo(void) {
if (micros < lastMicros) {
mavlinkData.nextHeartbeat = 0;
mavlinkData.nextParam = 0;
- mavlinkData.wpNext = 0;
+ mavlinkData.nextWP = 0;
for (uint8_t i=0; i < AQMAVLINK_TOTAL_STREAMS; ++i)
mavlinkData.streams[i].next = 0;
}
@@ -244,20 +245,6 @@ void mavlinkDo(void) {
if (!mavlinkData.customDatasets[i])
continue;
switch(i) {
- case AQMAV_DATASET_LEGACY1 :
- mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, AQ_ROLL, AQ_PITCH, AQ_YAW, IMU_RATEX, IMU_RATEY, IMU_RATEZ, IMU_ACCX, IMU_ACCY, IMU_ACCZ, IMU_MAGX, IMU_MAGY, IMU_MAGZ,
- navData.holdHeading, AQ_PRESSURE, IMU_TEMP, ALTITUDE, analogData.vIn, UKF_POSN, UKF_POSE, UKF_POSD);
- break;
- case AQMAV_DATASET_LEGACY2 :
- mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, gpsData.lat, gpsData.lon, gpsData.hAcc, gpsData.heading, gpsData.height, gpsData.pDOP,
- navData.holdCourse, navData.holdDistance, navData.holdAlt, navData.holdTiltN, navData.holdTiltE, UKF_VELN, UKF_VELE, VELOCITYD,
- RADIO_QUALITY, UKF_ACC_BIAS_X, UKF_ACC_BIAS_Y, UKF_ACC_BIAS_Z, 0, RADIO_ERROR_COUNT);
- break;
- case AQMAV_DATASET_LEGACY3 :
- mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, RADIO_THROT, RADIO_RUDD, RADIO_PITCH, RADIO_ROLL, rcGetControlValue(NAV_CTRL_PH), rcGetControlValue(NAV_CTRL_HOM_GO),
- motorsData.value[0], motorsData.value[1], motorsData.value[2], motorsData.value[3], motorsData.value[4], motorsData.value[5], motorsData.value[6],
- motorsData.value[7], motorsData.value[8], motorsData.value[9], motorsData.value[10], motorsData.value[11], motorsData.value[12], motorsData.value[13]);
- break;
case AQMAV_DATASET_GPS :
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, gpsData.pDOP, gpsData.tDOP, gpsData.sAcc, gpsData.cAcc, gpsData.velN, gpsData.velE, gpsData.velD,
gpsData.lastPosUpdate, gpsData.lastMessage, gpsData.vAcc, gpsData.lat, gpsData.lon, gpsData.hAcc, gpsData.heading, gpsData.height, gpsData.iTOW, 0,0,0,0);
@@ -268,12 +255,12 @@ void mavlinkDo(void) {
break;
case AQMAV_DATASET_SUPERVISOR :
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, supervisorData.state, supervisorData.flightTime, 0, 0,
- supervisorData.vInLPF, 0, supervisorData.lastGoodRadioMicros, supervisorData.idlePercent, 0,0,0,0,0,0,0,0,0, analogData.vIn, RADIO_QUALITY, RADIO_ERROR_COUNT);
+ supervisorData.vInLPF, 0, supervisorData.lastGoodRadioMicros, supervisorData.idlePercent, 0,0,0,0,0,0,0,0, commData.txBufStarved, analogData.vIn, RADIO_QUALITY, RADIO_ERROR_COUNT);
break;
case AQMAV_DATASET_STACKSFREE :
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, utilGetStackFree("INIT"), utilGetStackFree("FILER"), utilGetStackFree("SUPERVISOR"), utilGetStackFree("ADC"),
utilGetStackFree("RADIO"), utilGetStackFree("CONTROL"), utilGetStackFree("GPS"), utilGetStackFree("RUN"), utilGetStackFree("COMM"), utilGetStackFree("DIMU"),
- 0,0,0,0,0,0,0,0,0,0);
+ 0,0,0,0,0,0,0, heapUsed, heapHighWater, dataSramUsed);
break;
case AQMAV_DATASET_GIMBAL :
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, *gimbalData.pitchPort->ccr, *gimbalData.tiltPort->ccr, *gimbalData.rollPort->ccr, *gimbalData.triggerPort->ccr,
@@ -303,6 +290,12 @@ void mavlinkDo(void) {
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, AQ_ROLL, AQ_PITCH, AQ_YAW, IMU_RATEX, IMU_RATEY, IMU_RATEZ, IMU_ACCX, IMU_ACCY, IMU_ACCZ, IMU_MAGX, IMU_MAGY, IMU_MAGZ,
IMU_TEMP, 0, 0, 0, 0, 0, 0, AQ_PRESSURE);
break;
+ case AQMAV_DATASET_RC :
+ mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i, RADIO_THROT, RADIO_RUDD, RADIO_PITCH, RADIO_ROLL, rcIsSwitchActive(NAV_CTRL_AH), rcIsSwitchActive(NAV_CTRL_PH),
+ rcIsSwitchActive(NAV_CTRL_MISN), rcIsSwitchActive(NAV_CTRL_HOM_SET), rcIsSwitchActive(NAV_CTRL_HOM_GO), rcIsSwitchActive(NAV_CTRL_HF_SET), rcIsSwitchActive(NAV_CTRL_HF_LOCK),
+ rcIsSwitchActive(NAV_CTRL_WP_REC), 0, 0, 0, 0, 0,
+ rcIsSwitchActive(GMBL_CTRL_TRG_ON), rcGetControlValue(GMBL_CTRL_TILT), rcGetControlValue(GMBL_PSTHR_CHAN));
+ break;
case AQMAV_DATASET_DEBUG :
// First 10 values are displayed in QGC as integers, the other 10 as floats.
mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_0, i,
@@ -324,10 +317,10 @@ void mavlinkDo(void) {
}
// request announced waypoints from mission planner
- if (mavlinkData.wpCurrent < mavlinkData.wpCount && mavlinkData.wpAttempt <= AQMAVLINK_WP_MAX_ATTEMPTS && mavlinkData.wpNext < micros) {
+ if (mavlinkData.wpCurrent < mavlinkData.wpCount && mavlinkData.wpAttempt <= AQMAVLINK_WP_MAX_ATTEMPTS && mavlinkData.nextWP < micros) {
mavlinkData.wpAttempt++;
mavlink_msg_mission_request_send(MAVLINK_COMM_0, mavlinkData.wpTargetSysId, mavlinkData.wpTargetCompId, mavlinkData.wpCurrent);
- mavlinkData.wpNext = micros + AQMAVLINK_WP_TIMEOUT;
+ mavlinkData.nextWP = micros + AQMAVLINK_WP_TIMEOUT;
}
// or ack that last waypoint received
else if (mavlinkData.wpCurrent == mavlinkData.wpCount) {
@@ -461,21 +454,15 @@ void mavlinkDoCommand(mavlink_message_t *msg) {
param2 = mavlink_msg_command_long_get_param2(msg); // interval in us
param3 = mavlink_msg_command_long_get_param3(msg); // dataset id
- if (param3 < AQMAV_DATASET_ENUM_END) {
+ if (param3 >= AQMAV_DATASET_ALL && param3 < AQMAV_DATASET_ENUM_END) {
enable = (uint8_t)param;
// enable/disable this dataset
mavlinkData.customDatasets[(uint8_t)param3] = enable;
- // "legacy" diagnostic telemetry mode enables 2 more datasets
- if ((uint8_t)param3 == AQMAV_DATASET_LEGACY1) {
- mavlinkData.customDatasets[AQMAV_DATASET_LEGACY2] = enable;
- mavlinkData.customDatasets[AQMAV_DATASET_LEGACY3] = enable;
- }
-
// check if any datasets are active and enable/disable EXTRA3 stream accordingly
// AQMAV_DATASET_ALL is special and toggles all datasets
- if (!enable && (uint8_t)param3 != AQMAV_DATASET_ALL) {
+ if (!enable && param3 != AQMAV_DATASET_ALL) {
for (i=0; i < AQMAV_DATASET_ENUM_END; ++i)
if ((enable = mavlinkData.customDatasets[i]) > 0)
break;
@@ -488,6 +475,7 @@ void mavlinkDoCommand(mavlink_message_t *msg) {
mavlinkData.streams[MAV_DATA_STREAM_EXTRA3].interval = (unsigned long)param2;
ack = MAV_CMD_ACK_OK;
+ mavlink_msg_data_stream_send(MAVLINK_COMM_0, MAV_DATA_STREAM_EXTRA3, 1e6 / (unsigned long)param2, enable);
} else {
ack = MAV_CMD_ACK_ERR_FAIL;
@@ -519,7 +507,6 @@ void mavlinkDoCommand(mavlink_message_t *msg) {
void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
mavlink_message_t msg;
- char paramId[17];
uint8_t c;
// process incoming data
@@ -633,7 +620,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
mavlinkData.wpTargetCompId = msg.compid;
mavlinkData.wpCount = count;
mavlinkData.wpCurrent = mavlinkData.wpAttempt = 0;
- mavlinkData.wpNext = timerMicros();
+ mavlinkData.nextWP = timerMicros();
ack = 0;
}
@@ -673,7 +660,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
wp->targetRadius = mavlink_msg_mission_item_get_param1(&msg);
wp->loiterTime = mavlink_msg_mission_item_get_param2(&msg) * 1000;
wp->poiHeading = mavlink_msg_mission_item_get_param4(&msg);
- wp->maxHorizSpeed = p[NAV_MAX_SPEED];
+ wp->maxHorizSpeed = NAV_DFLT_HOR_SPEED;
}
else if (command == MAV_CMD_NAV_WAYPOINT) {
wp = navGetWaypoint(seqId);
@@ -707,7 +694,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
wp->targetRadius = mavlink_msg_mission_item_get_param1(&msg);
wp->loiterTime = mavlink_msg_mission_item_get_param2(&msg) * 1000;
wp->poiHeading = mavlink_msg_mission_item_get_param4(&msg);
- wp->maxHorizSpeed = p[NAV_MAX_SPEED];
+ wp->maxHorizSpeed = NAV_DFLT_HOR_SPEED;
}
}
else if (command == MAV_CMD_NAV_TAKEOFF) {
@@ -726,7 +713,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
wp->poiHeading = mavlink_msg_mission_item_get_param3(&msg);
wp->maxVertSpeed = mavlink_msg_mission_item_get_param4(&msg);
}
- else if (command == 1) { // TODO: stop using hard-coded values, use enums like intended
+ else if (command == MAV_CMD_AQ_NAV_LEG_ORBIT) {
wp = navGetWaypoint(seqId);
if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)
wp->relativeAlt = 1;
@@ -755,7 +742,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
wp->targetAlt = mavlink_msg_mission_item_get_z(&msg);
wp->maxVertSpeed = mavlink_msg_mission_item_get_param2(&msg);
wp->maxHorizSpeed = mavlink_msg_mission_item_get_param3(&msg);
- wp->poiAltitude = mavlink_msg_mission_item_get_param4(&msg);
+ wp->poiHeading = mavlink_msg_mission_item_get_param4(&msg);
}
else {
// NACK
@@ -765,7 +752,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
mavlinkData.wpCurrent = seqId + 1;
mavlinkData.wpAttempt = 0;
- mavlinkData.wpNext = timerMicros();
+ mavlinkData.nextWP = timerMicros();
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, mavlink_system.sysid, mavlink_system.compid, ack);
}
break;
@@ -791,15 +778,16 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
if (mavlink_msg_param_set_get_target_system(&msg) == mavlink_system.sysid) {
int paramIndex = -1;
float paramValue;
+ char paramName[17];
- mavlink_msg_param_set_get_param_id(&msg, paramId);
- paramIndex = configGetParamIdByName((char *)paramId);
+ mavlink_msg_param_set_get_param_id(&msg, paramName);
+ paramIndex = configGetParamIdByName((char *)paramName);
if (paramIndex >= 0 && paramIndex < CONFIG_NUM_PARAMS) {
paramValue = mavlink_msg_param_set_get_param_value(&msg);
if (!isnan(paramValue) && !isinf(paramValue) && !(supervisorData.state & STATE_FLYING))
p[paramIndex] = paramValue;
// send back what we have no matter what
- mavlink_msg_param_value_send(MAVLINK_COMM_0, paramId, p[paramIndex], MAVLINK_TYPE_FLOAT, CONFIG_NUM_PARAMS, paramIndex);
+ mavlink_msg_param_value_send(MAVLINK_COMM_0, paramName, p[paramIndex], MAVLINK_TYPE_FLOAT, CONFIG_NUM_PARAMS, paramIndex);
}
}
break;
@@ -850,7 +838,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
}
}
-void mavlinkSetSystemType(void) {
+static void mavlinkSetSystemType(void) {
uint8_t motCount = 0;
// get motor count
@@ -902,7 +890,7 @@ void mavlinkInit(void) {
mavlinkData.sys_mode = MAV_MODE_PREFLIGHT;
mavlinkData.sys_state = MAV_STATE_BOOT;
mavlink_system.sysid = flashSerno(0) % 250;
- mavlink_system.compid = MAV_COMP_ID_MISSIONPLANNER;
+ mavlink_system.compid = AQMAVLINK_DEFAULT_COMP_ID;
mavlinkSetSystemType();
mavlinkData.streams[MAV_DATA_STREAM_ALL].dfltInterval = AQMAVLINK_STREAM_RATE_ALL;
diff --git a/src/aq_mavlink.h b/src/aq_mavlink.h
index ae2e2e81..e8c27a2d 100644
--- a/src/aq_mavlink.h
+++ b/src/aq_mavlink.h
@@ -14,6 +14,7 @@
along with AutoQuad. If not, see .
Copyright © 2011-2014 Bill Nesbitt
+ Copyright 2013-2016 Maxim Paperno
*/
#ifndef _aq_mavlink_h
@@ -33,6 +34,7 @@
#define AQMAVLINK_PARAM_INTERVAL (1e6f / 150.0f) // 150Hz
#define AQMAVLINK_WP_TIMEOUT 1e6f // 1 second - retry frequency for waypoint requests to planner
#define AQMAVLINK_WP_MAX_ATTEMPTS 20 // maximum number of retries for wpnt. requests
+#define AQMAVLINK_DEFAULT_COMP_ID MAV_COMP_ID_MISSIONPLANNER
// this should equal MAV_DATA_STREAM_ENUM_END from mavlink.h
#define AQMAVLINK_TOTAL_STREAMS 14
@@ -49,10 +51,10 @@
#define AQMAVLINK_STREAM_RATE_PROPULSION 0 // ESC/Motor telemetry
enum mavlinkCustomDataSets {
- AQMAV_DATASET_LEGACY1 = 0, // legacy sets can eventually be phased out
- AQMAV_DATASET_LEGACY2,
- AQMAV_DATASET_LEGACY3,
- AQMAV_DATASET_ALL, // use this to toggle all datasets at once
+// AQMAV_DATASET_LEGACY1 = 0, // legacy sets can eventually be phased out
+// AQMAV_DATASET_LEGACY2,
+// AQMAV_DATASET_LEGACY3,
+ AQMAV_DATASET_ALL = 3, // use this to toggle all datasets at once
AQMAV_DATASET_GPS,
AQMAV_DATASET_UKF,
AQMAV_DATASET_SUPERVISOR,
@@ -63,40 +65,43 @@ enum mavlinkCustomDataSets {
AQMAV_DATASET_NAV,
AQMAV_DATASET_IMU,
AQMAV_DATASET_DEBUG,
+ AQMAV_DATASET_RC,
AQMAV_DATASET_ENUM_END
};
typedef struct {
- unsigned long interval; // how often to send stream in us (zero to disable)
- unsigned long dfltInterval; // default stream interval at startup
- unsigned long next; // when to send next stream data
- uint8_t enable; // enable/disable stream
+ uint32_t interval; // how often to send stream in us (zero to disable)
+ uint32_t dfltInterval; // default stream interval at startup
+ uint32_t next; // when to send next stream data
+ uint8_t enable; // enable/disable stream
} mavlinkStreams_t;
typedef struct {
- uint8_t sys_type; // System type (MAV_TYPE enum)
- uint8_t sys_state; // System state (MAV_STATE enum)
- uint8_t sys_mode; // System operating mode (MAV_MODE_FLAG enum bitmask)
-
mavlink_status_t mavlinkStatus;
mavlinkStreams_t streams[AQMAVLINK_TOTAL_STREAMS];
- uint8_t customDatasets[AQMAV_DATASET_ENUM_END];
- unsigned long nextHeartbeat;
- unsigned long nextParam;
- unsigned int currentParam;
+ uint32_t nextHeartbeat; // time when to send next heartbeat
+ uint32_t nextParam; // time when to send next param value
+ uint32_t nextWP; // when to send the next wpt request to planner
+ uint16_t currentParam; // keep track of parameter send sequence
uint16_t packetDrops; // global packet drop counter
- uint8_t indexPort; // current port # in channels outputs sequence
+ uint8_t sys_type; // System type (MAV_TYPE enum)
+ uint8_t sys_state; // System state (MAV_STATE enum)
+ uint8_t sys_mode; // System operating mode (MAV_MODE_FLAG enum bitmask)
+
+ uint8_t indexPort; // current port # in channels outputs sequence
+ uint8_t paramCompId; // component ID to use for params list
// waypoint programming from mission planner
uint8_t wpTargetSysId;
uint8_t wpTargetCompId;
uint8_t wpCount; // total waypoints to expect from planner after mission_count msg
uint8_t wpCurrent; // current wpt sequence # requested/expected from planner
- uint32_t wpNext; // when to send the next wpt request to planner
uint8_t wpAttempt; // count of consecutive wpt requests for same sequence #
+ uint8_t customDatasets[AQMAV_DATASET_ENUM_END];
+
} mavlinkStruct_t;
extern mavlinkStruct_t mavlinkData;
diff --git a/src/buildnum.h b/src/buildnum.h
index 58b28d36..76b4a25e 100644
--- a/src/buildnum.h
+++ b/src/buildnum.h
@@ -1 +1 @@
-#define BUILDNUMBER 1886
+#define BUILDNUMBER 1887