apm无人机主程序分析.docx
《apm无人机主程序分析.docx》由会员分享，可在线阅读，更多相关《apm无人机主程序分析.docx（12页珍藏版）》请在冰豆网上搜索。
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
*JonathanChallinger:
InertialNavigation
*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
*
*Requiresmodified"mrelax"versionofArduino,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
3f1cmusedtopointthenoseofthecopteratthenextwaypoint
staticint32_toriginal_wp_bearing;
staticuint32_twp_distance;
Thesevaluesaregenerated
Theyareusedthroughoutthecodewherecosandsin
Thisvalueisresetateacharming
20mstaticint32_tinitial_simple_bearing;
3fx=lat,y=lon
Incrementedatcircle_rate/second
staticfloatcircle_angle;
1.05fstaticint16_tsonar_alt;
staticuint8_tsonar_alt_health;staticint32_tnav_roll;
negativePitchmeansgoforward.
staticint32_tnav_pitch;
staticint32_tof_roll;
negativePitchmeansgoforward.
staticint32_tof_pitch;
staticint16_tnav_throttle;staticint32_tnav_yaw;
staticuint8_tyaw_timer;
3f
staticuint32_tcondition_start;
Usedforperformancemonitoringandfailsafeprocessing
staticuint16_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（）;//donotallowtheaccelbasedthrottletooverrideourcommand
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_throttleset_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_takeoff_complete（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_throttleset_throttle_out（pilot_throttle_scaled,true）;
//updateestimateofthrottlecruise
#ifFRAME_CONFIG==HELI_FRAME
update_throttle_cruise;
#else
update_throttle_cruise（pilot_throttle_scaled）;
#endif//HELI_FRAME
if（!
&&（））{
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_rateif（sonar_alt_health>=SONAR_ALT_HEALTH_MAX）{
//ifsonarisok,usesurfacetracking
get_throttle_surface_tracking（pilot_climb_rate）;//thisfunctioncallsset_target_alt_for_reportingforus
}else{
//ifnosonarfallbackstabilizeratecontroller
get_throttle_rate_stabilized（pilot_climb_rate）;//thisfunctioncallsset_target_alt_for_reportingforus
}
}else{
//pilot'sthrottlemustbeatzerosokeepmotorsoff
set_throttle_out（0,false）;
set_target_alt_for_reporting（0）;
}
break;
caseTHROTTLE_AUTO:
//autopilotaltitudecontrollerwithtargetaltitudeheldin（）
if{
get_throttle_althold_with_slew（）,（）,（））;
set_target_alt_for_reporting（））;//To-Do:
returnget_destination_altifweareflyingtoawaypoint
}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_for_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
（）;
omega=（）;
#ifSECONDARY_DMP_ENABLED==ENABLED
（）;
#endif
}
staticvoidupdate_trig（void）{
Vector2fyawvector;
constMatrix3f&temp=（）;
=//sin
=//cos
（）;
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:
//180=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）/;
//0to1
switch{
//Roll,Pitchtuning
caseCH6_STABILIZE_ROLL_PITCH_KP:
break;
caseCH6_RATE_ROLL_PITCH_KP:
break;
caseCH6_RATE_ROLL_PITCH_KI:
break;
caseCH6_RATE_ROLL_PITCH_KD:
break;
//Yawtuning
caseCH6_STABILIZE_YAW_KP:
break;
caseCH6_YAW_RATE_KP:
break;
caseCH6_YAW_RATE_KD:
break;
//Altitudeandthrottletuning
caseCH6_ALTITUDE_HOLD_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;
//Acroandothertuning
caseCH6_ACRO_KP:
=tuning_value;
break;
caseCH6_RELAY:
if>525）（）;
if<475）（）;
break;
#ifFRAME_CONFIG==HELI_FRAME
caseCH6_HELI_EXTERNAL_GYRO:
=tuning_value;
break;
#endif
caseCH6_OPTFLOW_KP:
break;
caseCH6_OPTFLOW_KI:
break;
caseCH6_OPTFLOW_KD:
break;
#ifHIL_MODE!
=HIL_MODE_ATTITUDE//donotallowmodifying_kpor_kp_yawgainsinHILmode
caseCH6_AHRS_YAW_KP:
break;
caseCH6_AHRS_KP:
break;
#endif
caseCH6_INAV_TC:
//To-Do:
allowingtuningTCforxyandzseparately
（tuning_value）;
（tuning_value）;
break;
caseCH6_DECLINATION:
//setdeclinationto+-20degrees
（ToRad（（2.0f*-/100.0f）,false）;//2ndparameterisfalsebecausewedonotwanttosavetoeeprombecausethiswouldhaveaperformanceimpact
break;
caseCH6_CIRCLE_RATE:
//setcirclerate
//allowapproximately45degreeturnrateineitherdirection
break;
}
}
AP_HAL_MAIN（）;
升级会员 