/* This file is part of AutoQuad. AutoQuad is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. AutoQuad is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with AutoQuad. If not, see <http://www.gnu.org/licenses/>. Copyright Â 2011 Bill Nesbitt*/#include"aq_mavlink.h"#include"mavlink.h"#include"config.h"#include"timer.h"#include"adc.h"#include"spektrum.h"#include"quat.h"#include"gps.h"#include"flash.h"#include"motors.h"#include"control.h"#include"nav.h"#include"math.h"#include<string.h>mavlinkStruct_tmavlinkData;mavlink_system_tmavlink_system;unsignedmavlinkSendTaskStack[1+STACKSIZE+1];unsignedmavlinkRecvTaskStack[1+STACKSIZE+1];voidcomm_send_ch(mavlink_channel_tchan,uint8_tch){serialWrite(mavlinkData.serialPort,ch);}voidmavlinkNotice(constchar*s){// queue message, notify and leavectl_message_queue_post_nb(&mavlinkData.notices,(void*)s);ctl_events_pulse(&publicEvents,AQ_TELEM_RDY);}voidmavlinkSendTaskCode(void*unused){serialPort_t*s=mavlinkData.serialPort;void*msg;longradioHits,radioMisses;unsignedlongmavCounter;unsignedlongmicros;unsignedlonglastMicros=0;AQ_NOTICE("Mavlink send task started...\n");while(1){ctl_events_wait(CTL_EVENT_WAIT_ANY_EVENTS,&publicEvents,AQ_TELEM_RDY,CTL_TIMEOUT_DELAY,100);// 10Hz minimummicros=timerMicros();mavCounter=counter;// handle rolloverif(micros<lastMicros){mavlinkData.nextHeartbeat=0;mavlinkData.nextParam=0;memset(mavlinkData.streamNext,0,sizeof(mavlinkData.streamInterval));}lastMicros=micros;// calculate radio reception qualityif(spektrumData.frameCount>radioHits)mavlinkData.radioQuality=(spektrumData.frameCount-radioHits)*255/((spektrumData.frameCount-radioHits)+(spektrumData.errorCount-radioMisses));elseif(mavlinkData.radioQuality)mavlinkData.radioQuality--;radioHits=spektrumData.frameCount;radioMisses=spektrumData.errorCount;// calculate idle timemavlinkData.idlePercent=(mavCounter-mavlinkData.lastCounter)*1000*(1e6/2/adcData.sampleTime)/(72e6/minCycles);mavlinkData.lastCounter=mavCounter;ctl_mutex_lock(&mavlinkData.serialPortMutex,CTL_TIMEOUT_NONE,0);// heartbeat & statusif(mavlinkData.nextHeartbeat<micros){mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,MAV_AUTOPILOT_GENERIC);mavlink_msg_sys_status_send(MAVLINK_COMM_0,mavlinkData.mode,mavlinkData.nav_mode,mavlinkData.status,1000-mavlinkData.idlePercent,AQ_VIN*1000,(AQ_VIN-9.8f)/12.6f*1000,mavlinkData.packetDrops);mavlinkData.nextHeartbeat=micros+MAVLINK_HEARTBEAT_INTERVAL;}// raw sensorselseif((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL]||mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_SENSORS])&&mavlinkData.streamNext[MAV_DATA_STREAM_RAW_SENSORS]<micros){mavlink_msg_scaled_imu_send(MAVLINK_COMM_0,micros,AQ_ACCX*1000.0,AQ_ACCY*1000.0,AQ_ACCZ*1000.0,AQ_RATEX*1000.0,AQ_RATEY*1000.0,AQ_RATEZ*1000.0,AQ_MAGX*1000.0,AQ_MAGY*1000.0,AQ_MAGZ*1000.0);mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0,micros,AQ_PRESSURE*0.01f,0.0f,AQ_TEMP*100);mavlinkData.streamNext[MAV_DATA_STREAM_RAW_SENSORS]=micros+mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_SENSORS];}// positionelseif((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL]||mavlinkData.streamInterval[MAV_DATA_STREAM_POSITION])&&mavlinkData.streamNext[MAV_DATA_STREAM_POSITION]<micros){mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,micros,3,gpsData.lat*1e7,gpsData.lon*1e7,gpsData.height*1e3,gpsData.hDOP,gpsData.vDOP,gpsData.speed,gpsData.heading);// mavlink_msg_gps_raw_send(MAVLINK_COMM_0, micros, 3, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hDOP, gpsData.vDOP, gpsData.speed, gpsData.heading);mavlinkData.streamNext[MAV_DATA_STREAM_POSITION]=micros+mavlinkData.streamInterval[MAV_DATA_STREAM_POSITION];}// extended status// else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_EXTENDED_STATUS]) && mavlinkData.streamNext[MAV_DATA_STREAM_EXTENDED_STATUS] < micros) {// mavlinkData.streamNext[MAV_DATA_STREAM_EXTENDED_STATUS] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_EXTENDED_STATUS];// }// rc channelselseif((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL]||mavlinkData.streamInterval[MAV_DATA_STREAM_RC_CHANNELS])&&mavlinkData.streamNext[MAV_DATA_STREAM_RC_CHANNELS]<micros){mavlink_msg_rc_channels_raw_send(MAVLINK_COMM_0,SPEKTRUM_THROT+1024,SPEKTRUM_ROLL+1024,SPEKTRUM_PITCH+1024,SPEKTRUM_RUDD+1024,SPEKTRUM_GEAR+1024,SPEKTRUM_FLAPS+1024,SPEKTRUM_AUX2+1024,SPEKTRUM_AUX3+1024,mavlinkData.radioQuality);mavlink_msg_rc_channels_scaled_send(MAVLINK_COMM_0,(SPEKTRUM_THROT-750)*13,SPEKTRUM_ROLL*13,SPEKTRUM_PITCH*13,SPEKTRUM_RUDD*13,SPEKTRUM_GEAR*13,SPEKTRUM_FLAPS*13,SPEKTRUM_AUX2*13,SPEKTRUM_AUX3*13,mavlinkData.radioQuality);mavlinkData.streamNext[MAV_DATA_STREAM_RC_CHANNELS]=micros+mavlinkData.streamInterval[MAV_DATA_STREAM_RC_CHANNELS];}// raw controllerelseif((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL]||mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_CONTROLLER])&&mavlinkData.streamNext[MAV_DATA_STREAM_RAW_CONTROLLER]<micros){mavlink_msg_attitude_send(MAVLINK_COMM_0,micros,-AQ_ROLL*DEG_TO_RAD,AQ_PITCH*DEG_TO_RAD,AQ_YAW*DEG_TO_RAD,-(AQ_RATEX-quatData.rateBias[0])*DEG_TO_RAD,(AQ_RATEY-quatData.rateBias[0])*DEG_TO_RAD,(AQ_RATEZ-quatData.rateBias[0])*DEG_TO_RAD);mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0,motorsData.m1,motorsData.m2,motorsData.m3,motorsData.m4,motorsData.m5,motorsData.m6,motorsData.m7,motorsData.m8);mavlinkData.streamNext[MAV_DATA_STREAM_RAW_CONTROLLER]=micros+mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_CONTROLLER];}// send pending noticesif(ctl_message_queue_receive_nb(&mavlinkData.notices,&msg)!=0)mavlink_msg_statustext_send(MAVLINK_COMM_0,0,(constint8_t*)msg);// list all parametersif(mavlinkData.currentParam<mavlinkData.numParams&&mavlinkData.nextParam<micros){mavlink_msg_param_value_send(MAVLINK_COMM_0,mavlinkData.paramIds[mavlinkData.currentParam],*mavlinkData.paramValues[mavlinkData.currentParam],mavlinkData.numParams,mavlinkData.currentParam++);mavlinkData.nextParam=micros+MABLINK_PARAM_INTERVAL;}ctl_mutex_unlock(&mavlinkData.serialPortMutex);}}voidmavlinkTakeAction(mavlink_message_t*msg){uint8_taction;uint8_tack=0;action=mavlink_msg_action_get_action(msg);switch(action){caseMAV_ACTION_STORAGE_READ:if(!controlData.flying){configFlashRead();ack=1;}break;caseMAV_ACTION_STORAGE_WRITE:if(!controlData.flying&&configFlashWrite())ack=1;break;default:break;}mavlink_msg_action_ack_send(MAVLINK_COMM_0,action,ack);}voidmavlinkRecvTaskCode(void*unused){serialPort_t*s=mavlinkData.serialPort;mavlink_message_tmsg;mavlink_status_tstatus;int8_tparamId[16];uint8_tc;AQ_NOTICE("Mavlink receive task started...\n");while(1){ctl_events_wait(CTL_EVENT_WAIT_ANY_EVENTS,&serialAvailableEvent,(1<<s->waitFlag),CTL_TIMEOUT_NONE,0);// process incoming datawhile(serialAvailable(s)){c=serialRead(s);// Try to get a new messageif(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status)){ctl_mutex_lock(&mavlinkData.serialPortMutex,CTL_TIMEOUT_NONE,0);digitalHi(mavlinkData.debugLed);// Handle messageswitch(msg.msgid){// TODO: finish this blockcaseMAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL:if(mavlink_msg_set_mode_get_target(&msg)==mavlink_system.sysid){mavlink_msg_change_operator_control_ack_send(MAVLINK_COMM_0,msg.sysid,mavlink_msg_change_operator_control_get_control_request(&msg),0);}break;caseMAVLINK_MSG_ID_SET_MODE:if(mavlink_msg_set_mode_get_target(&msg)==mavlink_system.sysid){mavlinkData.mode=mavlink_msg_set_mode_get_mode(&msg);mavlink_msg_sys_status_send(MAVLINK_COMM_0,mavlinkData.mode,mavlinkData.nav_mode,mavlinkData.status,mavlinkData.idlePercent,AQ_VIN*1000,500,mavlinkData.packetDrops);}break;caseMAVLINK_MSG_ID_ACTION:if(mavlink_msg_action_get_target(&msg)==mavlink_system.sysid){mavlinkTakeAction(&msg);}break;caseMAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:if(mavlink_msg_waypoint_request_list_get_target_system(&msg)==mavlink_system.sysid){mavlink_msg_waypoint_count_send(MAVLINK_COMM_0,msg.sysid,msg.compid,navGetWaypointCount());}break;caseMAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:if(mavlink_msg_waypoint_clear_all_get_target_system(&msg)==mavlink_system.sysid){mavlink_msg_waypoint_ack_send(MAVLINK_COMM_0,mavlink_system.sysid,mavlink_system.compid,navClearWaypoints());}break;caseMAVLINK_MSG_ID_WAYPOINT_REQUEST:if(mavlink_msg_waypoint_request_get_target_system(&msg)==mavlink_system.sysid){uint16_tseqId;navMission_t*wp;seqId=mavlink_msg_waypoint_request_get_seq(&msg);wp=navGetWaypoint(seqId);mavlink_msg_waypoint_send(MAVLINK_COMM_0,msg.sysid,msg.compid,seqId,MAV_FRAME_GLOBAL,MAV_CMD_NAV_WAYPOINT,0,1,wp->targetRadius,wp->loiterTime,0.0,wp->poiHeading,wp->targetLat,wp->targetLon,wp->targetAlt);}break;caseMAVLINK_MSG_ID_WAYPOINT_COUNT:if(mavlink_msg_waypoint_count_get_target_system(&msg)==mavlink_system.sysid){mavlink_msg_waypoint_ack_send(MAVLINK_COMM_0,mavlink_system.sysid,mavlink_system.compid,0);}break;caseMAVLINK_MSG_ID_PARAM_REQUEST_READ:if(mavlink_msg_param_request_read_get_target_system(&msg)==mavlink_system.sysid){uint16_tparamIndex;paramIndex=mavlink_msg_param_request_read_get_param_index(&msg);if(paramIndex<mavlinkData.numParams)mavlink_msg_param_value_send(MAVLINK_COMM_0,mavlinkData.paramIds[paramIndex],*mavlinkData.paramValues[paramIndex],mavlinkData.numParams,paramIndex);}break;caseMAVLINK_MSG_ID_PARAM_REQUEST_LIST:if(mavlink_msg_param_request_list_get_target_system(&msg)==mavlink_system.sysid){mavlinkData.currentParam=0;mavlinkData.nextParam=0;}break;caseMAVLINK_MSG_ID_PARAM_SET:if(mavlink_msg_param_set_get_target_system(&msg)==mavlink_system.sysid){intparamIndex=-1;floatparamValue;inti,j;mavlink_msg_param_set_get_param_id(&msg,paramId);for(i=0;i<mavlinkData.numParams;i++){for(j=0;j<MAVLINK_PARAMID_LEN;j++)if(paramId[j]!=mavlinkData.paramIds[i][j]||paramId[j]==0)break;if(paramId[j]==0){paramIndex=i;break;}}if(paramIndex>=0&&paramIndex<mavlinkData.numParams){paramValue=mavlink_msg_param_set_get_param_value(&msg);if(!isnan(paramValue)&&!isinf(paramValue)&&!controlData.flying)*mavlinkData.paramValues[i]=paramValue;// send back what we have no matter whatmavlink_msg_param_value_send(MAVLINK_COMM_0,paramId,*mavlinkData.paramValues[i],mavlinkData.numParams,i);}}break;caseMAVLINK_MSG_ID_REQUEST_DATA_STREAM:if(mavlink_msg_request_data_stream_get_target_system(&msg)==mavlink_system.sysid){uint16_trate;uint8_tstream_id;stream_id=mavlink_msg_request_data_stream_get_req_stream_id(&msg);rate=mavlink_msg_request_data_stream_get_req_message_rate(&msg);if(rate>0&&rate<200&&stream_id<MAV_DATA_STREAM_ENUM_END&&mavlink_msg_request_data_stream_get_start_stop(&msg)){mavlinkData.streamInterval[stream_id]=1e6/rate;}else{mavlinkData.streamInterval[stream_id]=0;}}break;default://Do nothingbreak;}digitalLo(mavlinkData.debugLed);ctl_mutex_unlock(&mavlinkData.serialPortMutex);}// Update global packet drops countermavlinkData.packetDrops+=status.packet_rx_drop_count;}}}intmavlinkInitParams(void){inti=0;mavlinkData.paramIds[i]="CTRL_FACT_THRO";mavlinkData.paramValues[i]=&configData.controlThroFactor;i++;mavlinkData.paramIds[i]="CTRL_FACT_PITC";mavlinkData.paramValues[i]=&configData.controlPitchFactor;i++;mavlinkData.paramIds[i]="CTRL_FACT_ROLL";mavlinkData.paramValues[i]=&configData.controlRollFactor;i++;mavlinkData.paramIds[i]="CTRL_FACT_RUDD";mavlinkData.paramValues[i]=&configData.controlRuddFactor;i++;mavlinkData.paramIds[i]="CTRL_DEAD_BAND";mavlinkData.paramValues[i]=&configData.controlDeadBand;i++;mavlinkData.paramIds[i]="CTRL_MIN_THROT";mavlinkData.paramValues[i]=&configData.controlMinThro;i++;mavlinkData.paramIds[i]="CTRL_MAX";mavlinkData.paramValues[i]=&configData.controlMax;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_P";mavlinkData.paramValues[i]=&configData.controlTiltRateP;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_I";mavlinkData.paramValues[i]=&configData.controlTiltRateI;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_D";mavlinkData.paramValues[i]=&configData.controlTiltRateD;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_F";mavlinkData.paramValues[i]=&configData.controlTiltRateF;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_PM";mavlinkData.paramValues[i]=&configData.controlTiltRateMaxP;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_IM";mavlinkData.paramValues[i]=&configData.controlTiltRateMaxI;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_DM";mavlinkData.paramValues[i]=&configData.controlTiltRateMaxD;i++;mavlinkData.paramIds[i]="CTRL_TLT_RTE_OM";mavlinkData.paramValues[i]=&configData.controlTiltRateMaxO;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_P";mavlinkData.paramValues[i]=&configData.controlYawRateP;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_I";mavlinkData.paramValues[i]=&configData.controlYawRateI;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_D";mavlinkData.paramValues[i]=&configData.controlYawRateD;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_F";mavlinkData.paramValues[i]=&configData.controlYawRateF;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_PM";mavlinkData.paramValues[i]=&configData.controlYawRateMaxP;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_IM";mavlinkData.paramValues[i]=&configData.controlYawRateMaxI;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_DM";mavlinkData.paramValues[i]=&configData.controlYawRateMaxD;i++;mavlinkData.paramIds[i]="CTRL_YAW_RTE_OM";mavlinkData.paramValues[i]=&configData.controlYawRateMaxO;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_P";mavlinkData.paramValues[i]=&configData.controlTiltAngleP;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_I";mavlinkData.paramValues[i]=&configData.controlTiltAngleI;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_D";mavlinkData.paramValues[i]=&configData.controlTiltAngleD;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_F";mavlinkData.paramValues[i]=&configData.controlTiltAngleF;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_PM";mavlinkData.paramValues[i]=&configData.controlTiltAngleMaxP;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_IM";mavlinkData.paramValues[i]=&configData.controlTiltAngleMaxI;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_DM";mavlinkData.paramValues[i]=&configData.controlTiltAngleMaxD;i++;mavlinkData.paramIds[i]="CTRL_TLT_ANG_OM";mavlinkData.paramValues[i]=&configData.controlTiltAngleMaxO;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_P";mavlinkData.paramValues[i]=&configData.controlYawAngleP;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_I";mavlinkData.paramValues[i]=&configData.controlYawAngleI;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_D";mavlinkData.paramValues[i]=&configData.controlYawAngleD;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_F";mavlinkData.paramValues[i]=&configData.controlYawAngleF;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_PM";mavlinkData.paramValues[i]=&configData.controlYawAngleMaxP;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_IM";mavlinkData.paramValues[i]=&configData.controlYawAngleMaxI;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_DM";mavlinkData.paramValues[i]=&configData.controlYawAngleMaxD;i++;mavlinkData.paramIds[i]="CTRL_YAW_ANG_OM";mavlinkData.paramValues[i]=&configData.controlYawAngleMaxO;i++;mavlinkData.paramIds[i]="GPS_BAUD_RATE";mavlinkData.paramValues[i]=&configData.gpsBaud;i++;mavlinkData.paramIds[i]="GPS_POS_DEV";mavlinkData.paramValues[i]=&configData.gpsPosDev;i++;mavlinkData.paramIds[i]="GPS_VEL_DEV";mavlinkData.paramValues[i]=&configData.gpsVelDev;i++;mavlinkData.paramIds[i]="GPS_RATE";mavlinkData.paramValues[i]=&configData.gpsRate;i++;mavlinkData.paramIds[i]="MOT_START";mavlinkData.paramValues[i]=&configData.motorsStart;i++;mavlinkData.paramIds[i]="MOT_MIN";mavlinkData.paramValues[i]=&configData.motorsMin;i++;mavlinkData.paramIds[i]="MOT_MAX";mavlinkData.paramValues[i]=&configData.motorsMax;i++;mavlinkData.paramIds[i]="MOT_HOV_THROT";mavlinkData.paramValues[i]=&configData.motorsHoverThrot;i++;mavlinkData.paramIds[i]="MOT_EXP_FACT";mavlinkData.paramValues[i]=&configData.motorsExpFactor;i++;mavlinkData.paramIds[i]="MOT_EXP_MIN";mavlinkData.paramValues[i]=&configData.motorsExpMin;i++;mavlinkData.paramIds[i]="MOT_EXP_MAX";mavlinkData.paramValues[i]=&configData.motorsExpMax;i++;mavlinkData.paramIds[i]="MOT_ROLL_ASYM";mavlinkData.paramValues[i]=&configData.motorsRollAsym;i++;mavlinkData.paramIds[i]="MOT_PITCH_ASYM";mavlinkData.paramValues[i]=&configData.motorsPitchAsym;i++;mavlinkData.paramIds[i]="MOT_MAG_BIAS_X";mavlinkData.paramValues[i]=&configData.motorsMagBiasX;i++;mavlinkData.paramIds[i]="MOT_MAG_BIAS_Y";mavlinkData.paramValues[i]=&configData.motorsMagBiasY;i++;mavlinkData.paramIds[i]="MOT_MAG_BIAS_Z";mavlinkData.paramValues[i]=&configData.motorsMagBiasZ;i++;mavlinkData.paramIds[i]="MOT_MAG_SCAL_X";mavlinkData.paramValues[i]=&configData.motorsMagScaleX;i++;mavlinkData.paramIds[i]="MOT_MAG_SCAL_Y";mavlinkData.paramValues[i]=&configData.motorsMagScaleY;i++;mavlinkData.paramIds[i]="MOT_MAG_SCAL_Z";mavlinkData.paramValues[i]=&configData.motorsMagScaleZ;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_XY";mavlinkData.paramValues[i]=&configData.motorsMagAlgnXY;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_XZ";mavlinkData.paramValues[i]=&configData.motorsMagAlgnXZ;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_YX";mavlinkData.paramValues[i]=&configData.motorsMagAlgnYX;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_YZ";mavlinkData.paramValues[i]=&configData.motorsMagAlgnYZ;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_ZX";mavlinkData.paramValues[i]=&configData.motorsMagAlgnZX;i++;mavlinkData.paramIds[i]="MOT_MAG_ALGN_ZY";mavlinkData.paramValues[i]=&configData.motorsMagAlgnZY;i++;mavlinkData.paramIds[i]="DOWNLINK_BAUD";mavlinkData.paramValues[i]=&configData.downlinkBaud;i++;mavlinkData.paramIds[i]="TELEMETRY_RATE";mavlinkData.paramValues[i]=&configData.telemetryRate;i++;mavlinkData.paramIds[i]="NAV_MIN_PDOP";mavlinkData.paramValues[i]=&configData.navMinPDOP;i++;mavlinkData.paramIds[i]="NAV_MAX_FIXAGE";mavlinkData.paramValues[i]=&configData.navMaxFixAge;i++;mavlinkData.paramIds[i]="NAV_MAX_TILT";mavlinkData.paramValues[i]=&configData.navMaxTilt;i++;mavlinkData.paramIds[i]="NAV_MAX_SPEED";mavlinkData.paramValues[i]=&configData.navMaxSpeed;i++;mavlinkData.paramIds[i]="NAV_SPEED_P";mavlinkData.paramValues[i]=&configData.navSpeedP;i++;mavlinkData.paramIds[i]="NAV_SPEED_I";mavlinkData.paramValues[i]=&configData.navSpeedI;i++;mavlinkData.paramIds[i]="NAV_SPEED_PM";mavlinkData.paramValues[i]=&configData.navSpeedMaxP;i++;mavlinkData.paramIds[i]="NAV_SPEED_IM";mavlinkData.paramValues[i]=&configData.navSpeedMaxI;i++;mavlinkData.paramIds[i]="NAV_SPEED_OM";mavlinkData.paramValues[i]=&configData.navSpeedMaxO;i++;mavlinkData.paramIds[i]="NAV_DIST_P";mavlinkData.paramValues[i]=&configData.navDistP;i++;mavlinkData.paramIds[i]="NAV_DIST_I";mavlinkData.paramValues[i]=&configData.navDistI;i++;mavlinkData.paramIds[i]="NAV_DIST_PM";mavlinkData.paramValues[i]=&configData.navDistMaxP;i++;mavlinkData.paramIds[i]="NAV_DIST_IM";mavlinkData.paramValues[i]=&configData.navDistMaxI;i++;mavlinkData.paramIds[i]="NAV_DIST_OM";mavlinkData.paramValues[i]=&configData.navDistMaxO;i++;mavlinkData.paramIds[i]="NAV_ATL_SPED_P";mavlinkData.paramValues[i]=&configData.navAltSpeedP;i++;mavlinkData.paramIds[i]="NAV_ATL_SPED_I";mavlinkData.paramValues[i]=&configData.navAltSpeedI;i++;mavlinkData.paramIds[i]="NAV_ATL_SPED_PM";mavlinkData.paramValues[i]=&configData.navAltSpeedMaxP;i++;mavlinkData.paramIds[i]="NAV_ATL_SPED_IM";mavlinkData.paramValues[i]=&configData.navAltSpeedMaxI;i++;mavlinkData.paramIds[i]="NAV_ATL_SPED_OM";mavlinkData.paramValues[i]=&configData.navAltSpeedMaxO;i++;mavlinkData.paramIds[i]="NAV_ALT_POS_P";mavlinkData.paramValues[i]=&configData.navAltPosP;i++;mavlinkData.paramIds[i]="NAV_ALT_POS_I";mavlinkData.paramValues[i]=&configData.navAltPosI;i++;mavlinkData.paramIds[i]="NAV_ALT_POS_PM";mavlinkData.paramValues[i]=&configData.navAltPosMaxP;i++;mavlinkData.paramIds[i]="NAV_ALT_POS_IM";mavlinkData.paramValues[i]=&configData.navAltPosMaxI;i++;mavlinkData.paramIds[i]="NAV_ALT_POS_OM";mavlinkData.paramValues[i]=&configData.navAltPosMaxO;i++;mavlinkData.paramIds[i]="NAV_FILTER_PX";mavlinkData.paramValues[i]=&configData.navFiltPx;i++;mavlinkData.paramIds[i]="NAV_FILTER_VX";mavlinkData.paramValues[i]=&configData.navFiltVx;i++;mavlinkData.paramIds[i]="NAV_FILTER_BX";mavlinkData.paramValues[i]=&configData.navFiltBx;i++;mavlinkData.paramIds[i]="NAV_FILTER_PZ";mavlinkData.paramValues[i]=&configData.navFiltPz;i++;mavlinkData.paramIds[i]="NAV_FILTER_VZ";mavlinkData.paramValues[i]=&configData.navFiltVz;i++;mavlinkData.paramIds[i]="NAV_FILTER_BZ";mavlinkData.paramValues[i]=&configData.navFiltBz;i++;mavlinkData.paramIds[i]="IMU_ROT";mavlinkData.paramValues[i]=&configData.adcIMURot;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS_X";mavlinkData.paramValues[i]=&configData.adcAccBiasX;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS_Y";mavlinkData.paramValues[i]=&configData.adcAccBiasY;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS_Z";mavlinkData.paramValues[i]=&configData.adcAccBiasZ;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS1_X";mavlinkData.paramValues[i]=&configData.adcAccBias1X;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS1_Y";mavlinkData.paramValues[i]=&configData.adcAccBias1Y;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS1_Z";mavlinkData.paramValues[i]=&configData.adcAccBias1Z;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS2_X";mavlinkData.paramValues[i]=&configData.adcAccBias2X;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS2_Y";mavlinkData.paramValues[i]=&configData.adcAccBias2Y;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS2_Z";mavlinkData.paramValues[i]=&configData.adcAccBias2Z;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS3_X";mavlinkData.paramValues[i]=&configData.adcAccBias3X;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS3_Y";mavlinkData.paramValues[i]=&configData.adcAccBias3Y;i++;mavlinkData.paramIds[i]="IMU_ACC_BIAS3_Z";mavlinkData.paramValues[i]=&configData.adcAccBias3Z;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL_X";mavlinkData.paramValues[i]=&configData.adcAccScaleX;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL_Y";mavlinkData.paramValues[i]=&configData.adcAccScaleY;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL_Z";mavlinkData.paramValues[i]=&configData.adcAccScaleZ;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL1_X";mavlinkData.paramValues[i]=&configData.adcAccScale1X;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL1_Y";mavlinkData.paramValues[i]=&configData.adcAccScale1Y;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL1_Z";mavlinkData.paramValues[i]=&configData.adcAccScale1Z;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL2_X";mavlinkData.paramValues[i]=&configData.adcAccScale2X;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL2_Y";mavlinkData.paramValues[i]=&configData.adcAccScale2Y;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL2_Z";mavlinkData.paramValues[i]=&configData.adcAccScale2Z;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL3_X";mavlinkData.paramValues[i]=&configData.adcAccScale3X;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL3_Y";mavlinkData.paramValues[i]=&configData.adcAccScale3Y;i++;mavlinkData.paramIds[i]="IMU_ACC_SCAL3_Z";mavlinkData.paramValues[i]=&configData.adcAccScale3Z;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_XY";mavlinkData.paramValues[i]=&configData.adcAccAlgnXY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_XZ";mavlinkData.paramValues[i]=&configData.adcAccAlgnXZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_YX";mavlinkData.paramValues[i]=&configData.adcAccAlgnYX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_YZ";mavlinkData.paramValues[i]=&configData.adcAccAlgnYZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_ZX";mavlinkData.paramValues[i]=&configData.adcAccAlgnZX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALGN_ZY";mavlinkData.paramValues[i]=&configData.adcAccAlgnZY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_XY";mavlinkData.paramValues[i]=&configData.adcAccAlgn1XY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_XZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn1XZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_YX";mavlinkData.paramValues[i]=&configData.adcAccAlgn1YX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_YZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn1YZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_ZX";mavlinkData.paramValues[i]=&configData.adcAccAlgn1ZX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN1_ZY";mavlinkData.paramValues[i]=&configData.adcAccAlgn1ZY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_XY";mavlinkData.paramValues[i]=&configData.adcAccAlgn2XY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_XZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn2XZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_YX";mavlinkData.paramValues[i]=&configData.adcAccAlgn2YX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_YZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn2YZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_ZX";mavlinkData.paramValues[i]=&configData.adcAccAlgn2ZX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN2_ZY";mavlinkData.paramValues[i]=&configData.adcAccAlgn2ZY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_XY";mavlinkData.paramValues[i]=&configData.adcAccAlgn3XY;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_XZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn3XZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_YX";mavlinkData.paramValues[i]=&configData.adcAccAlgn3YX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_YZ";mavlinkData.paramValues[i]=&configData.adcAccAlgn3YZ;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_ZX";mavlinkData.paramValues[i]=&configData.adcAccAlgn3ZX;i++;mavlinkData.paramIds[i]="IMU_ACC_ALN3_ZY";mavlinkData.paramValues[i]=&configData.adcAccAlgn3ZY;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS_X";mavlinkData.paramValues[i]=&configData.adcMagBiasX;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS_Y";mavlinkData.paramValues[i]=&configData.adcMagBiasY;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS_Z";mavlinkData.paramValues[i]=&configData.adcMagBiasZ;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS1_X";mavlinkData.paramValues[i]=&configData.adcMagBias1X;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS1_Y";mavlinkData.paramValues[i]=&configData.adcMagBias1Y;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS1_Z";mavlinkData.paramValues[i]=&configData.adcMagBias1Z;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS2_X";mavlinkData.paramValues[i]=&configData.adcMagBias2X;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS2_Y";mavlinkData.paramValues[i]=&configData.adcMagBias2Y;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS2_Z";mavlinkData.paramValues[i]=&configData.adcMagBias2Z;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS3_X";mavlinkData.paramValues[i]=&configData.adcMagBias3X;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS3_Y";mavlinkData.paramValues[i]=&configData.adcMagBias3Y;i++;mavlinkData.paramIds[i]="IMU_MAG_BIAS3_Z";mavlinkData.paramValues[i]=&configData.adcMagBias3Z;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL_X";mavlinkData.paramValues[i]=&configData.adcMagScaleX;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL_Y";mavlinkData.paramValues[i]=&configData.adcMagScaleY;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL_Z";mavlinkData.paramValues[i]=&configData.adcMagScaleZ;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL1_X";mavlinkData.paramValues[i]=&configData.adcMagScale1X;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL1_Y";mavlinkData.paramValues[i]=&configData.adcMagScale1Y;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL1_Z";mavlinkData.paramValues[i]=&configData.adcMagScale1Z;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL2_X";mavlinkData.paramValues[i]=&configData.adcMagScale2X;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL2_Y";mavlinkData.paramValues[i]=&configData.adcMagScale2Y;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL2_Z";mavlinkData.paramValues[i]=&configData.adcMagScale2Z;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL3_X";mavlinkData.paramValues[i]=&configData.adcMagScale3X;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL3_Y";mavlinkData.paramValues[i]=&configData.adcMagScale3Y;i++;mavlinkData.paramIds[i]="IMU_MAG_SCAL3_Z";mavlinkData.paramValues[i]=&configData.adcMagScale3Z;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_XY";mavlinkData.paramValues[i]=&configData.adcMagAlgnXY;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_XZ";mavlinkData.paramValues[i]=&configData.adcMagAlgnXZ;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_YX";mavlinkData.paramValues[i]=&configData.adcMagAlgnYX;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_YZ";mavlinkData.paramValues[i]=&configData.adcMagAlgnYZ;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_ZX";mavlinkData.paramValues[i]=&configData.adcMagAlgnZX;i++;mavlinkData.paramIds[i]="IMU_MAG_ALGN_ZY";mavlinkData.paramValues[i]=&configData.adcMagAlgnZY;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS_X";mavlinkData.paramValues[i]=&configData.adcGyoBiasX;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS_Y";mavlinkData.paramValues[i]=&configData.adcGyoBiasY;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS_Z";mavlinkData.paramValues[i]=&configData.adcGyoBiasZ;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS1_X";mavlinkData.paramValues[i]=&configData.adcGyoBias1X;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS1_Y";mavlinkData.paramValues[i]=&configData.adcGyoBias1Y;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS1_Z";mavlinkData.paramValues[i]=&configData.adcGyoBias1Z;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS2_X";mavlinkData.paramValues[i]=&configData.adcGyoBias2X;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS2_Y";mavlinkData.paramValues[i]=&configData.adcGyoBias2Y;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS2_Z";mavlinkData.paramValues[i]=&configData.adcGyoBias2Z;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS3_X";mavlinkData.paramValues[i]=&configData.adcGyoBias3X;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS3_Y";mavlinkData.paramValues[i]=&configData.adcGyoBias3Y;i++;mavlinkData.paramIds[i]="IMU_GYO_BIAS3_Z";mavlinkData.paramValues[i]=&configData.adcGyoBias3Z;i++;mavlinkData.paramIds[i]="IMU_GYO_SCAL_X";mavlinkData.paramValues[i]=&configData.adcGyoScaleX;i++;mavlinkData.paramIds[i]="IMU_GYO_SCAL_Y";mavlinkData.paramValues[i]=&configData.adcGyoScaleY;i++;mavlinkData.paramIds[i]="IMU_GYO_SCAL_Z";mavlinkData.paramValues[i]=&configData.adcGyoScaleZ;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_XY";mavlinkData.paramValues[i]=&configData.adcGyoAlgnXY;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_XZ";mavlinkData.paramValues[i]=&configData.adcGyoAlgnXZ;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_YX";mavlinkData.paramValues[i]=&configData.adcGyoAlgnYX;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_YZ";mavlinkData.paramValues[i]=&configData.adcGyoAlgnYZ;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_ZX";mavlinkData.paramValues[i]=&configData.adcGyoAlgnZX;i++;mavlinkData.paramIds[i]="IMU_GYO_ALGN_ZY";mavlinkData.paramValues[i]=&configData.adcGyoAlgnZY;i++;mavlinkData.paramIds[i]="QUAT_MAG_INCL";mavlinkData.paramValues[i]=&configData.quatMagIncl;i++;mavlinkData.paramIds[i]="QUAT_ACC_DIST";mavlinkData.paramValues[i]=&configData.quatAccDist;i++;mavlinkData.paramIds[i]="QUAT_KP";mavlinkData.paramValues[i]=&configData.quatKp;i++;mavlinkData.paramIds[i]="QUAT_KI";mavlinkData.paramValues[i]=&configData.quatKi;i++;mavlinkData.paramIds[i]="QUAT_KA";mavlinkData.paramValues[i]=&configData.quatKa;i++;mavlinkData.paramIds[i]="QUAT_KM1";mavlinkData.paramValues[i]=&configData.quatKm1;i++;mavlinkData.paramIds[i]="QUAT_KM2";mavlinkData.paramValues[i]=&configData.quatKm2;i++;returni;}voidmavlinkInit(void){unsignedlongmicros;inti;mavlinkData.serialPort=serialOpen(MAVLINK_USART,configData.downlinkBaud,USART_HardwareFlowControl_RTS_CTS);mavlinkData.debugLed=digitalInit(MAVLINK_LED_PORT,MAVLINK_LED_PIN);// setup mutex for serial port accessctl_mutex_init(&mavlinkData.serialPortMutex);ctl_message_queue_init(&mavlinkData.notices,mavlinkData.noticeQueue,MAVLINK_NOTICE_DEPTH);AQ_NOTICE("Mavlink init... ");mavlinkData.numParams=mavlinkInitParams();mavlinkData.currentParam=mavlinkData.numParams;// start send thread with high priority so that it can process startup notices - will drop priority latermemset(mavlinkSendTaskStack,0xcd,sizeof(mavlinkSendTaskStack));// write known values into the stackmavlinkSendTaskStack[0]=mavlinkSendTaskStack[1+STACKSIZE]=0xfacefeed;// put marker values at the words before/after the stackctl_task_run(&mavlinkData.sendTask,175,mavlinkSendTaskCode,0,"mavlinkSendTask",STACKSIZE,mavlinkSendTaskStack+1,0);memset(mavlinkRecvTaskStack,0xcd,sizeof(mavlinkRecvTaskStack));// write known values into the stackmavlinkRecvTaskStack[0]=mavlinkRecvTaskStack[1+STACKSIZE]=0xfacefeed;// put marker values at the words before/after the stackctl_task_run(&mavlinkData.recvTask,20,mavlinkRecvTaskCode,0,"mavlinkRecvTask",STACKSIZE,mavlinkRecvTaskStack+1,0);mavlink_system.sysid=flashSerno()%250;mavlink_system.compid=MAV_COMP_ID_WAYPOINTPLANNER;mavlink_system.type=MAV_QUADROTOR;mavlinkData.mode=MAV_MODE_MANUAL;mavlinkData.nav_mode=MAV_NAV_GROUNDED;mavlinkData.status=MAV_STATE_ACTIVE;// turn on all streams at 1Hz & spread them outmicros=timerMicros();for(i=1;i<13;i++){// TODO: remove hardcoded valuemavlinkData.streamInterval[i]=1e6;mavlinkData.streamNext[i]=micros+5e6+i*5e3;}}