diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index bec4ac0a5d..1d950edb35 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -8,8 +8,11 @@ // version 2.1 of the License, or (at your option) any later version. #include +#include #include "AP_PitchController.h" +extern const AP_HAL::HAL& hal; + const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { AP_GROUPINFO("AP", 0, AP_PitchController, _kp_angle, 1.0), AP_GROUPINFO("FF", 1, AP_PitchController, _kp_ff, 0.4), @@ -24,7 +27,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize) { - uint32_t tnow = millis(); + uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; if (_last_t == 0 || dt > 1000) { @@ -52,8 +55,11 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab int32_t desired_rate = angle_err * _kp_angle; - if (_max_rate_neg && desired_rate < -_max_rate_neg) desired_rate = -_max_rate_neg; - else if (_max_rate_pos && desired_rate > _max_rate_pos) desired_rate = _max_rate_pos; + if (_max_rate_neg && desired_rate < -_max_rate_neg) { + desired_rate = -_max_rate_neg; + } else if (_max_rate_pos && desired_rate > _max_rate_pos) { + desired_rate = _max_rate_pos; + } desired_rate *= roll_scaler; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 3dd6552fbe..c3264e2afb 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -1,9 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_PitchController_h -#define AP_PitchController_h +#ifndef __AP_PITCH_CONTROLLER_H__ +#define __AP_PITCH_CONTROLLER_H__ -#include #include #include #include // for fabs() @@ -42,4 +41,4 @@ private: static const uint8_t _fCut = 20; }; -#endif +#endif // __AP_PITCH_CONTROLLER_H__ diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 89cfcbe82e..2499c80716 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -8,9 +8,12 @@ // version 2.1 of the License, or (at your option) any later version. #include +#include #include "AP_RollController.h" +extern const AP_HAL::HAL& hal; + const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { AP_GROUPINFO("AP", 0, AP_RollController, _kp_angle, 1.0), AP_GROUPINFO("FF", 1, AP_RollController, _kp_ff, 0.4), @@ -23,7 +26,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize) { - uint32_t tnow = millis(); + uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; if (_last_t == 0 || dt > 1000) { dt = 0; diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index dd2cf8c8f4..2565d08333 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -1,9 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_RollController_h -#define AP_RollController_h +#ifndef __AP_ROLL_CONTROLLER_H__ +#define __AP_ROLL_CONTROLLER_H__ -#include #include #include #include // for fabs() @@ -40,4 +39,4 @@ private: static const uint8_t _fCut = 20; }; -#endif +#endif // __AP_ROLL_CONTROLLER_H__ diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 396510b736..03acdc4984 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -8,9 +8,11 @@ // version 2.1 of the License, or (at your option) any later version. #include - +#include #include "AP_YawController.h" +extern const AP_HAL::HAL& hal; + const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { AP_GROUPINFO("P", 0, AP_YawController, _kp, 0), AP_GROUPINFO("I", 1, AP_YawController, _ki, 0), @@ -20,7 +22,7 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) { - uint32_t tnow = millis(); + uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; if (_last_t == 0 || dt > 1000) { dt = 0; @@ -46,8 +48,10 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) Vector3f accels = _ins->get_accel(); - // I didn't pull 512 out of a hat - it is a (very) loose approximation of 100*ToDeg(asin(-accels.y/9.81)) - // which, with a P of 1.0, would mean that your rudder angle would be equal to your roll angle when + // I didn't pull 512 out of a hat - it is a (very) loose approximation of + // 100*ToDeg(asin(-accels.y/9.81)) + // which, with a P of 1.0, would mean that your rudder angle would be + // equal to your roll angle when // the plane is still. Thus we have an (approximate) unit to go by. float error = 512 * -accels.y; @@ -58,8 +62,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) _last_error = error; // integrator if(_freeze_start_time < (tnow - 2000)) { - if ((fabs(_ki) > 0) && (dt > 0)) - { + if ((fabs(_ki) > 0) && (dt > 0)) { _integrator += (error * _ki) * scaler * delta_time; if (_integrator < -_imax) _integrator = -_imax; else if (_integrator > _imax) _integrator = _imax; diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 693db2021f..b735cadb9a 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -1,9 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_YawController_h -#define AP_YawController_h +#ifndef __AP_YAW_CONTROLLER_H__ +#define __AP_YAW_CONTROLLER_H__ -#include #include #include #include // for fabs() @@ -42,4 +41,4 @@ private: static const float _fCut = FCUT(.5); }; -#endif +#endif // __AP_YAW_CONTROLLER_H__