apm无人机主程序分析Word下载.docx
- 文档编号:16299060
- 上传时间:2022-11-22
- 格式:DOCX
- 页数:12
- 大小:17.81KB
apm无人机主程序分析Word下载.docx
《apm无人机主程序分析Word下载.docx》由会员分享,可在线阅读,更多相关《apm无人机主程序分析Word下载.docx(12页珍藏版)》请在冰豆网上搜索。
*JonathanChallinger:
InertialNavigation
*Jean-LouisNaudin:
AutoLanding
*MaxLevine:
TriSupport,Graphics
*JackDunkle:
*JamesGoppert:
MavlinkSupport
*JaniHiriven:
Testingfeedback
*JohnArneBirkeland:
PPMEncoder
*JoseJulio:
StabilizationControllaws
*MarcoRobustini:
Leadtester
*MichaelOborne:
MissionPlannerGCS
*MikeSmith:
Libraries,Codingsupport
*Oliver:
Piezosupport
*OlivierAdler:
*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;
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;
staticint32_tof_pitch;
staticint16_tnav_throttle;
staticint32_tnav_yaw;
staticuint8_tyaw_timer;
3f
staticuint32_tcondition_start;
Usedforperformancemonitoringandfailsafeprocessing
staticuint16_tmainLoop_count;
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){
}else{
//sendpilot'
soutputdirectlytomotors
pilot_throttle_scaled=get_pilot_desired_throttleset_throttle_out(pilot_throttle_scaled,false);
//updateestimateofthrottlecruise
#ifFRAME_CONFIG==HELI_FRAME
update_throttle_cruise;
update_throttle_cruise(pilot_throttle_scaled);
#endif//HELI_FRAME
//checkifwe'
vetakenoffyet
if(!
&
&
()){
if(pilot_throttle_scaled>
{
//wemustbeintheairbynow
set_takeoff_complete(true);
break;
caseTHROTTLE_MANUAL_TILT_COMPENSATED:
//manualthrottlebutwithangleboost
if<
=0){
//noneedforangleboostwithzerothrottle
pilot_throttle_scaled=get_pilot_desired_throttleset_throttle_out(pilot_throttle_scaled,true);
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
//ifnosonarfallbackstabilizeratecontroller
get_throttle_rate_stabilized(pilot_climb_rate);
//pilot'
sthrottlemustbeatzerosokeepmotorsoff
caseTHROTTLE_AUTO:
//autopilotaltitudecontrollerwithtargetaltitudeheldin()
get_throttle_althold_with_slew(),(),());
set_target_alt_for_reporting());
//To-Do:
returnget_destination_altifweareflyingtoawaypoint
caseTHROTTLE_LAND:
//landingthrottlecontroller
get_throttle_land();
}
//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
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°
//180=cos_yaw:
//270=cos_yaw:
//readbaroandsonaraltitudeat20hz
staticvoidupdate_altitude()
#ifHIL_MODE==HIL_MODE_ATTITUDE
//weareintheSIM,fakeoutthebaroandSonar
baro_alt=g_gps->
altitude_cm-gps_base_alt;
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:
caseCH6_RATE_ROLL_PITCH_KI:
caseCH6_RATE_ROLL_PITCH_KD:
//Yawtuning
caseCH6_STABILIZE_YAW_KP:
break;
caseCH6_YAW_RATE_KP:
caseCH6_YAW_RATE_KD:
//Altitudeandthrottletuning
caseCH6_ALTITUDE_HOLD_KP:
caseCH6_THROTTLE_RATE_KP:
caseCH6_THROTTLE_RATE_KD:
caseCH6_THROTTLE_ACCEL_KP:
caseCH6_THROTTLE_ACCEL_KI:
caseCH6_THROTTLE_ACCEL_KD:
//Loiterandnavigationtuning
caseCH6_LOITER_POSITION_KP:
caseCH6_LOITER_RATE_KP:
caseCH6_LOITER_RATE_KI:
caseCH6_LOITER_RATE_KD:
caseCH6_WP_SPEED:
//setwaypointnavigationhorizontalspeedto0~1000cm/s
//Acroandothertuning
caseCH6_ACRO_KP:
=tuning_value;
caseCH6_RELAY:
if>
525)();
475)();
caseCH6_HELI_EXTERNAL_GYRO:
caseCH6_OPTFLOW_KP:
caseCH6_OPTFLOW_KI:
caseCH6_OPTFLOW_KD:
=HIL_MODE_ATTITUDE//donotallowmodifying_kpor_kp_yawgainsinHILmode
caseCH6_AHRS_YAW_KP:
caseCH6_AHRS_KP:
caseCH6_INAV_TC:
allowingtuningTCforxyandzseparately
(tuning_value);
caseCH6_DECLINATION:
//setdeclinationto+-20degrees
(ToRad((2.0f*-/100.0f),false);
//2ndparameterisfalsebecausewedonotwanttosavetoeeprombecausethiswouldhaveaperformanceimpact
caseCH6_CIRCLE_RATE:
//setcirclerate
//allowapproximately45degreeturnrateineitherdirection
AP_HAL_MAIN();
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- apm 无人机 主程序 分析
![提示](https://static.bdocx.com/images/bang_tan.gif)