Skip to content

Commit

Permalink
aq_mavlink: NOTE latest headers required, update /lib/mavlink submodule;
Browse files Browse the repository at this point in the history 
Fix setting of desired yaw/heading for Landing type waypoint;
Add RC dataset to diagnostic telemetry;
Add RAM usage and comm buffer errors to diagnostics;
Remove legacy diagnostic datasets;
Optimize RAM usage of mavlinkData struct.
  • Loading branch information
@mpaperno
mpaperno committed Feb 21, 2016
1 parent aa4910a commit 39f4461
Show file tree
Hide file tree
Showing 5 changed files with 62 additions and 62 deletions.
9 changes: 8 additions & 1 deletion CHANGES.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion lib/mavlink
Submodule mavlink updated 3 files
+24 −5 include/autoquad/autoquad.h
+1 −1 include/autoquad/version.h
+1 −1 include/common/version.h
70 changes: 29 additions & 41 deletions src/aq_mavlink.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011-2014 Bill Nesbitt
Copyright 2013-2016 Maxim Paperno
*/

#include "aq.h"
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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) {
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -850,7 +838,7 @@ void mavlinkRecvTaskCode(commRcvrStruct_t *r) {
}
}

void mavlinkSetSystemType(void) {
static void mavlinkSetSystemType(void) {
uint8_t motCount = 0;

// get motor count
Expand Down Expand Up @@ -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;
Expand Down
41 changes: 23 additions & 18 deletions src/aq_mavlink.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011-2014 Bill Nesbitt
Copyright 2013-2016 Maxim Paperno
*/

#ifndef _aq_mavlink_h
Expand All @@ -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
Expand All @@ -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,
Expand All @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/buildnum.h
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
#define BUILDNUMBER 1886
#define BUILDNUMBER 1887

0 comments on commit 39f4461

Please sign in to comment.