diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 297a2d20ab..6b5ea38ea9 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -4,30 +4,31 @@ // Run landing gear controller at 10Hz void Copter::landinggear_update() { - // If landing gear control is active, run update function. - if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ + // exit immediately if no landing gear switch is enabled + if (!check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)) { + return; + } - // last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear - static bool last_deploy_status = landinggear.deployed(); + // last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear + static bool last_deploy_status = landinggear.deployed(); - // if we are doing an automatic landing procedure, force the landing gear to deploy. - // To-Do: should we pause the auto-land procedure to give time for gear to come down? - if (control_mode == LAND || - (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || - (control_mode == AUTO && auto_mode == Auto_Land) || - (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { - landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); - } + // if we are doing an automatic landing procedure, force the landing gear to deploy. + // To-Do: should we pause the auto-land procedure to give time for gear to come down? + if (control_mode == LAND || + (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || + (control_mode == AUTO && auto_mode == Auto_Land) || + (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { + landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); + } - // send event message to datalog if status has changed - if (landinggear.deployed() != last_deploy_status){ - if (landinggear.deployed()) { - Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); - } else { - Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); - } + // send event message to datalog if status has changed + if (landinggear.deployed() != last_deploy_status) { + if (landinggear.deployed()) { + Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); + } else { + Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); } - - last_deploy_status = landinggear.deployed(); } + + last_deploy_status = landinggear.deployed(); }