apm无人机主程序分析

3.0.1*

*SpecialThanksforContributors(inalphabeticalorderbyfirstname):

*

*AdamMRivera:AutoCompassDeclination

*AmilcarLucas:Cameramountlibrary

*AndrewTridgell:Generaldevelopment,MavlinkSupport

*AngelFernandez:Alphatesting

*DougWeibel:Libraries

*ChristofSchmid:Alphatesting

*DaniSaez:VOctoSupport

*GregoryFletcher:Cameramountorientationmath

*Guntars:Armingsafetysuggestion

*HappyKillmore:MavlinkGCS

*HeinHollander:OctoSupport

*IgorvanAirde:ControlLawoptimization

*LeonardHall:FlightDynamics,Throttle,LoiterandNavigationControllers

*JonathanChallingerInertialNavigation

*Jean-LouisNaudin:AutoLanding

*MaxLevine:TriSupport,Graphics

*JackDunkle:Alphatesting

*JamesGoppert:MavlinkSupport

*JaniHiriven:Testingfeedback

*JohnArneBirkeland:PPMEncoder

*JoseJulio:StabilizationControllaws

*MarcoRobustini:Leadtester

*MichaelOborne:MissionPlannerGCS

*MikeSmith:Libraries,Codingsupport

*Oliver:Piezosupport

*OlivierAdler:PPMEncoder

*RobertLefebvre:HeliSupport&LEDs

*SandroBenigno:Camerasupport

*

*AndmuchmoresoPLEASEPMmeonDIYDRONEStoaddyourcontributiontotheList

*

*RequiresmodifiednmrelaxnversionofArduino,whichcanbefoundhere:

**

*/

Itmay

staticAP_HAL::BetterStream*cliSerial;

weneedtokeepastaticdeclarationwhichisn'tguardedbymacros

Realsensorsareused.

Mostsensorsaredisabled,astheHIL

Syntheticsensorsareconfiguredthat

staticGPS*g_gps;

2C2c#endif

#endif

#ifCONFIG_HAL_BOARD=HAL_BOARD_PX4

staticAP_Compass_PX4compass;

#else

staticAP_Compass_HMC5843compass;

#endif

#endif

19g#endif#endif*Seelibraries/RC_Channel/formoreinformation

*/

It'scurrentlytherawIMUrates

3flcm;

usedtopointthenoseofthecopteratthenextwaypoint

staticint32_toriginal_wp_bearing;

staticuint32_twp_distance;

Thesevaluesaregenerated

Theyareusedthroughoutthecodewherecosandsin

Thisvalueisresetateacharming

20mstaticint32_tinitial_simple_bearing;

3fx二lat,y=Ion

Incrementedatcircle_rate/second

staticfloatcircle_angle;

1.05fstaticintl6_tsonar_alt;

staticuint8_tsonar_alt_health;staticint32_tnav_roll;

negativePitchmeansgoforward.

staticint32_tnav_pitch;

staticint32_toLroll;

negativePitchmeansgoforward.

staticint32_to匚pitch;

staticintl6_tnav_throttle;staticint32_tnav_yaw;

staticuint8_tyaw_timer;

3f

staticuint32_tcondition_start;

Usedforperformancemonitoringandfailsafeprocessing

staticuintl6_tmainLoop_count;

staticAP_HAL::AnalogSource*rssi_analog_source;

=false;

Log_Write_Event(DATA_EXIT_FLIP);

)

)

get_look_ahead_yawbreak;

#ifTOY_LOOKUP==TOY_EXTERNAL_MIXER

caseYAW_TOY:

controller_desired_alt=get_initial_alt_hold,climb_rate);

return;

//donotrunthrottlecontrollersifmotorsdisarmed

if(!()){

set_throttle_out(0,false);

throttle_accel_deactivate();//donotallowtheaccelbasedthrottletooverrideour

command

set_target_alt_for_reporting(0);

return;

)

#ifFRAME_CONFIG==HELI_FRAME

if(control_mode==STABILIZE)(

=true;

}else{

=false;

)

#endif//HELI_FRAME

switch(throttle_mode){

caseTHROTTLE_MANUAL:

//completelymanualthrottle

if<=0){

set_throttle_out(0,false);

}else{

//sendpilot'soutputdirectlytomotors

pilot_throttle_scaled=get_pilot_desired_throttle

set_throttle_out(pilot_throttle_scaled,false);

//updateestimateofthrottlecruise

#ifFRAME_CONFIG==HELI_FRAME

update_throttle_cruise;

#else

update_throttle_cruise(pilot_throttle_scaled);

#endif//HELI_FRAME

//checkifwe*vetakenoffyet

if(!&&()){

if(pilot_throttle_scaled>{

//wemustbeintheairbynow

set_takeoffLcomplete(true);

)

)

)

set_target_alt_for_reporting(0);

break;

caseTHROTTLE_MANUAL_TILT_COMPENSATED:

//manualthrottlebutwithangleboost

if<=0){

set_throttle_out(0,false);//noneedforangleboostwithzerothrottle

}else{

pilot_throttle_scaled=get_pilot_desired_throttle

set_throttle_out(pilot_throttle_scaled,true);

//updateestimateofthrottlecruise

#ifFRAME_CONHG==HELI_FRAME

update_throttle_cruise;

#else

update_throttle_cruise(pilot_throttle_scaled);

#endif//HELI_FRAME

ifC&&()){

if(pilot_throttle_scaled>{

//wemustbeintheairbynow

set_takeoff_complete(true);

)

)

set_target_alt_for_reporting(0);

break;

caseTHROTTLE_HOLD:

if{

//altholdpluspilotinputofclimbrate

pilot_climb_rate=get_pilot_desired_climb_rate

if(sonar_alt_health>=SONAR_ALT_HEALTH_MAX){

//ifsonarisok,usesurfacetracking

get_throttle_surface_tracking(pilot_climb_rate);//thisfunctioncalls

set_target_alt_for_reportingforus

}else{

//ifnosonarfallbackstabilizeratecontroller

get_throttle_rate_stabilized(pilot_climb_rate);//thisfunctioncalls

set_target_alt_for_reportingforus

)

}else{

//pilotsthrottlemustbeatzerosokeepmotorsoff

set_throttle_out(0,false);

set_target_alt_fdr_reporting(0);

)

break;

caseTHROTTLE_AUTO:

//autopilotaltitudecontrollerwithtargetaltitudeheldin()

if{

get_throttle_althold_with_slew(),(),());

set_target_alt_fdr_reporting());//To-Do:returnget_destination_altifweareflying

toawaypoint

}else{

//pilot'sthrottlemustbeatzerosokeepmotorsoff

set_throttle_out(0,false);

set_target_alt_for_reporting(0);

)

break;

caseTHROTTLE_LAND:

//landingthrottlecontroller

get_throttle_land();

set_target_alt_for_reporting(0);

break;

)

//set_target_alt_fbr_reporting-settargetaltitudeincmforreportingpurposes(logsandgcs)

staticvoidset_target_alt_for_reporting(floatalt_cm)

target_alt_for_reporting=alt_cm;

)

//get_target_alt_for_reporting-returnstargetaltitudeincmforreportingpurposes(logsandgcs)

staticfloatget_target_alt_for_reporting()

(

returntarget_alt_for_reporting;

)

staticvoidread_AHRS(void)

(

//PerformIMUcalculationsandgetattitudeinfo

//-------------------------------------------------

#ifHIL_MODE!=HIL_MODE_DISABLED

//updatehilbeforeahrsupdate

gcs_check_input();

#endif

0;

omega=();

#ifSECONDARY_DMP_ENABLED二二ENABLED

0;

#endif

)

staticvoidupdate_trig(void){

Vector2fyawvector;

constMatrix3f&temp=();

=//sin

=//cos

0;

cos_pitch_x=safe_sqrt(l-*//level=1

cos_roll_x=/cos_pitch_x;//level=1

cos_pitch_x=constrain_float(cos_pitch_x,0,;

//thisreliesonconstrain_float()ofinfinitydoingtherightthing,

//whichitdoesdoinavr-libc

cos_roll_x=constrain_float(cos_roll_x,,;

sin_yaw=constrain_float,,;

cos_yaw=constrain_float,,;

//addedtoconvertearthframetobodyframeforratecontrollers

sin_pitch=sin_roll=/cos_pitch_x;

//updatewp_navcontrollerwithtrigvalues

(cos_yaw,sin_yaw,cos_pitch_x);

//flat:

//0°=cos_yaw:,sin_yaw:,

//90°=cos_yaw:,sin_yaw:,

II180=cos_yaw:,sin_yaw:,

〃270=cos_yaw:,sin_yaw:,

)

//readbaroandsonaraltitudeat20hz

staticvoidupdate_altitude()

(

#ifHIL_MODE==HIL_MODE_ATTITUDE

//weareintheSIM,fakeoutthebaroandSonar

baro_alt=g_gps->altitude_cm-gps_base_alt;

if{

sonar_alt=baro_alt;

)

#else

//readinbaroaltitude

baro_alt=read_barometer();

//readinsonaraltitude

sonar_alt=read_sonar();

#endif//HIL_MODE==HIL_MODE_ATTITUDE

//writealtitudeinfotodataflashlogs

if(&MASK_LOG_CTUN)&&()){

Log_Write_Control_Tuning();

staticvoidtuning(){

tuning_value=(float)/;

//Oto1

switch{

//Roll,Pitchtuning

caseCH6_STABILIZE_ROLL_PITCH_KP:

break;

caseCH6_RATE_ROLL_PITCH_KP:

break;

caseCH6_RATE_ROLL_PITCH_KI:

break;

caseCH6_RATE_R0LL_PITCH_KD:

break;

//Yawtuning

caseCH6_STABILIZE_YAW_KP:

break;

caseCH6_YAW_RATE_KP:

break;

caseCH6_YAW_RATE_KD:

break;

//Altitudeandthrottletuning

caseCH6_ALTITUDE_H0LD_KP:

break;

caseCH6_THROTTLE_RATE_KP:

break;

caseCH6_THROTTLE_RATE_KD:

break;

caseCH6_THROTTLE_ACCEL_KP:

break;

caseCH6_THROTTLE_ACCEL_KI:

break;

caseCH6_THROTTLE_ACCEL_KD:

break;

//Loiterandnavigationtuning

caseCH6_LOITER_POSITION_KP:

break;

caseCH6_LOITER_RATE_KP:

break;

caseCH6_LOITER_RATE_KI:

break;

caseCH6_LOITER_RATE_KD:

break;

caseCH6_WP_SPEED:

//setwaypointnavigationhorizontalspeedto0〜1000cm/s

break;

//Aeroandothertuning

caseCH6_ACRO_KP:

=tuning_value;

break;

caseCH6_RELA