Carving out Control Code: Automated Identification of Control Software in Autopilot Systemsn

By Balaji Balasubramaniam , Iftekhar Ahmed , Hamid Bagheri , and Justin Bradley
Kansas State University - Salina, University of California - Irvine, University of Nebraska - Lincoln

Abstract

Cyber-physical systems interact with the world through software controlling physical effectors. Carefully designed controllers, implemented as safety-critical control software, also interact with other parts of the software suite, and may be difficult to separate, verify, or maintain. Moreover, some software changes, not intended to impact control system performance, do change controller response through a variety of means including interaction with external libraries or unmodeled changes only existing in the cyber system (e.g., exception handling). As a result, identifying safety-critical control software, its boundaries with other embedded software in the system, and the way in which control software evolves could help developers isolate, test, and verify control implementation, and improve control software development. In this work we present an automated technique, based on a novel application of machine learning, to detect commits related to control software, its changes, and how the control software evolves. We leverage messages from developers (e.g., commit comments), and code changes themselves to understand how control software is refined, extended, and adapted over time. We examine three distinct, popular, real-world, safety-critical autopilots – ArduPilot, Paparazzi UAV, and LibrePilot to test our method demonstrating an effective detection rate of 0.95 for control-related code changes

Category definitions

We develop an automated framework to 1) identify control related commits (In version control systems, such as Git, SVN, a commit is an operation which sends the latest software code changes of the original source code to the software repository, each such change is logged by commit IDs that are identified by unique hash values. This commit ID is created whenever a new commit is recorded and provides traceability benefits. Hence, we use this commit as our fundamental unit for our categorization), and 2) identify maintenance changes/commits that do not impact the control software. For this purpose we define two types of commits/changes:

Non-control commits

A software change that have no impact on the controller. These changes are either for another part of the system software, or are software maintenance changes such as documentation, refactoring, or abstraction.

This commit adds watchdog as a monitoring service. Developers initialize watchdog to catch faults. In the code snippet below, code additions are marked as '+' in the line beginning, and code deletions are marked as '-' in the line beginning.
Project & ID: LibrePilot 9934, GitHub link: 84465ae0cb File location: flight/modules/Attitude/attitude.c Committers comment: OP-1743 - Fix a very ugly error in attitude(cc/cc3d) 
	
...	
    // Start main task
    xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle);
+ #ifdef PIOS_INCLUDE_WDG
    PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
-
+ #endif
    return 0;
}
...

Control commits

A software change that impacts control software and performance.

We highlight a deceptively simple code change, the renaming of Airspeed to TrueAirspeed (True airspeed is the idealized airspeed of an aircraft used for planning purposes) in the pathplanner.c file. However, in reality, this single commit changed a total of 11 files with 55 code additions (Code additions are marked as '+' in the beginning lines of the code snippet below.) and 38 code deletions (Code deletions are marked as '-' in the beginning lines of the code snippet below.). Based on the developer’s comment and code change, one may be tempted to label this code change as non-control as it is a simple rename. However, upon further inspection, based on the probing questions approach, in the same commit, we find that there is another file called baroairspeed.xml in which the data for this variable is actually changed. In this case, the baroairspeed.xml file already had both the Airspeed parameter and TrueAirspeed parameter. After the code change, the developers renamed Airspeed to CalibratedAirspeed (Calibrated airspeed” is indicated, or measured airspeed corrected for instrument and positional errors) in baroairspeed.xml file. But in the pathplanner.c file calculation, however, they renamed Airspeed to TrueAirspeed. Hence, the velocity calculation end result is impacted because the data is potentially significantly different. Project & ID: LibrePilot 6229, GitHub link: aa5e5a9b01 File location: flight/Modules/PathPlanner/pathplanner.c Committers comment: Removed "airspeed" field from Airspeed UAVO. Now everything is done either with true airspeed or calibrated airspeed 
	
...	
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONECTED_TRUE) {
- 	velocity = baroAirspeed.Airspeed;
+ 	velocity = baroAirspeed.TrueAirspeed;
...
With these definitions in mind, we address the following research questions: RQ1: How well can we automatically identify the control related commits within projects using machine learning approaches? RQ2: Which factors are useful in determining the control commits? RQ3: Is cross-project training beneficial in predicting control commits?

Dataset

The prime aim of the project selection was to: (1) analyse autopilot system that is representative of code developed in the real world, (2) include significant safety-critical control code changes with many available versions reflecting their software development process, and (3) support reproducibility of the results obtained from our methodology. Therefore, we only select active, open-source autopilot software projects from GitHub. In addition, we opted to select projects that use programming languages C/ C++ for two reasons: first, embedded systems widely use C or C++, and second, “compiled” languages are extremely efficient as they are fast and stable making them often more suitable for low-level control.

We began by selecting two autopilot software projects - ArduPilot and Paparazzi UAV . We analyzed both the UAV projects that have more than ten years of development history and can operate a variety of vehicles including airplanes, multi-rotors, helicopters, and boats. These repositories have 448 and 99 contributors respectively. Also, they contain ∼38,000 and ∼15,400 code changes respectively. The latest version contains approximately 441k Lines Of Code (LOC) and 1.3M LOC, respectively.

To test the cross-project evaluation of our proposed automatic classification, we selected a third project – also widely used autopilot software – LibrePilot . It provides a sophisticated control system that supports various flight modes and can operate on a variety of vehicles including multi-rotors, fixed wing, and cars. It has over 11 years of well maintained history, and its code base is accessible through a git repository that stores the code changes committed by the developers since June, 2010. As of October 2021, the repository includes 66 contributors that have committed almost 15,000 changes. The latest version of LibrePilot contains approximately 452k lines of code (LOC) in C/C++.

About the below table:

The prefix ArduPilot, Paparazzi, and LibrePilot in "Project & ID" column represents the dataset artifacts ArduPilot , Paparazzi UAV , and LibrePilot respectively. The ID is a unique identifier to each commits in the controller files, to uniquely identify it.

  • Filename column - contains the file name where the commit has occured.
  • Category column - contains the category name to which a commit belongs to - either Control or Non-Control.
  • GitHub link column - includes a link to access the actual commit that is present in git version control system.
  • Commit message column - contains the comment provided by the developer.
  • Code changed column - contains the code changes happened for this commit.
Tips:
  • Use search for seeing all the commits belonging to a particular dataset. The search functionality searches the given text in all columns, however since the dataset name is unique you can view all the commits belonging to a particular dataset by entering thea name of the dataset. For example, enter "ArduPilot" to view all the commits belonging to ArduPilot dataset.
  • Towards the end of the table, you can use the grey box shaped filter by left clicking once. The first filter allows user to select commits from a specific control file. The second filter allows user to select commits belonging to a category.

Porject & ID Filename Category GitHub link Commit message Code changed
Filters: Filename Category GitHub link Commit message Code changed
ArduPilot 2 AC_PID.cpp Non-Control f9eed9bd9e fixed missing return in D value " return 0 "
ArduPilot 3 AC_PID.cpp Control 452749149f convert AC_PID library to AP_Param " const AP_Param::GroupInfo AC_PID::var_info PROGMEM = AP_GROUPINFO P AC_PID _kp AP_GROUPINFO I AC_PID _ki AP_GROUPINFO D AC_PID _kd AP_GROUPINFO IMAX AC_PID _imax AP_GROUPEND _kp.load _ki.load _kd.load _imax.load _kp.save _ki.save _kd.save _imax.save "
ArduPilot 4 AC_PID.cpp Non-Control 9c5a5473ab added indexes to group info structures " AP_GROUPINFO P 0 AC_PID _kp AP_GROUPINFO I 1 AC_PID _ki AP_GROUPINFO D 2 AC_PID _kd AP_GROUPINFO IMAX 3 AC_PID _imax "
ArduPilot 5 AC_PID.cpp Non-Control 24ce02c6a4 made I term return in same pattern as D term " return _integrator return 0 _derivative = error - _last_error / dt _last_error = error "
ArduPilot 6 AC_PID.cpp Control 24a471ff8f AC_PID - added more paranoid checking that imax is positive in constructor operator() and load_gains methods " _imax = abs _imax "
ArduPilot 7 AC_PID.cpp Control 5e8fe8d93e AP_Param: update PID libraries for new constructor syntax " AP_GROUPINFO P 0 AC_PID _kp 0 AP_GROUPINFO I 1 AC_PID _ki 0 AP_GROUPINFO D 2 AC_PID _kd 0 AP_GROUPINFO IMAX 3 AC_PID _imax 0 "
ArduPilot 8 AC_PID.cpp Non-Control fc9f1a8216 uncrustify libraries/AC_PID/AC_PID.cpp " AP_GROUPINFO P 0 AC_PID _kp 0 AP_GROUPINFO I 1 AC_PID _ki 0 AP_GROUPINFO D 2 AC_PID _kd 0 AP_GROUPINFO IMAX 3 AC_PID _imax 0 AP_GROUPEND return float error * _kp if _ki != 0 && dt != 0 _integrator += float error * _ki * dt if _integrator < -_imax _integrator = -_imax else if _integrator > _imax _integrator = _imax return _integrator return 0 if _kd != 0 && dt != 0 _derivative = input - _last_input / dt _derivative = _last_derivative + dt / _filter + dt * _derivative - _last_derivative _last_input = input _last_derivative = _derivative return _kd * _derivative return 0 return get_p error + get_i error dt return get_p error + get_i error dt + get_d error dt * int32_t AC_PID::get_pid int32_t error float dt * * * _output = error * _kp * * * if fabs _kd > 0 && dt > 0 * _derivative = error - _last_error / dt * * * * _derivative = _last_derivative + * dt / _filter + dt * _derivative - _last_derivative * * * _last_error = error * _last_derivative = _derivative * * * _output += _kd * _derivative * * * * if fabs _ki > 0 && dt > 0 * _integrator += error * _ki * dt * if _integrator < -_imax * _integrator = -_imax * else if _integrator > _imax * _integrator = _imax * * _output += _integrator * * * return _output * */ _integrator = 0 _last_input = 0 _last_derivative = 0 _kp.load _ki.load _kd.load _imax.load _imax = abs _imax _kp.save _ki.save _kd.save _imax.save "
ArduPilot 9 AC_PID.cpp Control 1d12b781a0 AC_PID: Create Leaky Integrator Function. " int32_t AC_PID::get_leaky_i int32_t error float dt float leak_rate if _ki != 0 && dt != 0 _integrator -= float _integrator * leak_rate _integrator += float error * _ki * dt if _integrator < -_imax _integrator = -_imax else if _integrator > _imax _integrator = _imax return _integrator return 0 "
ArduPilot 10 AC_PID.cpp Non-Control eac1ad49d6 AC_PID: save 8 bytes per AC_PID objectwe don t need the _derivative or _output variables " float derivative = input - _last_input / dt derivative = _last_derivative + dt / _filter + dt * derivative - _last_derivative _last_derivative = derivative return _kd * derivative "
ArduPilot 11 AC_PID.cpp Control 5e7ca5b0f8 AC_PID: suppress the derivative immediately after resetuse _last_derivative == NAN to flag that the derivative is invalid " float derivative if isnan _last_derivative derivative = 0 _last_derivative = 0 else derivative = input - _last_input / dt _last_derivative = NAN "
ArduPilot 12 AC_PID.cpp Non-Control 24317e721b AC_PID AP_AHRS: added descriptions to some parameters ""
ArduPilot 13 AC_PID.cpp Control 2294acc652 AC_PID: fixed build on ARM " const float AC_PID::_filter = 7.9577e-3 "
ArduPilot 14 AC_PID.cpp Control 4fa7bb1486 Add AVR compatibility header for missing math.h definitions.- Define float versions of math functions to the double versions on AVR (eg. #define sinf sin).- These macros appear to be missing in older versions of avr-libs.- Include AP_Math.h rather than math.h to get these definitions. " #include "
ArduPilot 15 AC_PID.cpp Non-Control 8c25a504c3 AC_PID: fixed indent-tabs-mode ""
ArduPilot 16 AC_PID.cpp Control fbc5af2705 AC_PID: results returned as float " float AC_PID::get_p float error const float AC_PID::get_i float error float dt float AC_PID::get_leaky_i float error float dt float leak_rate float AC_PID::get_d float input float dt dt / AC_PID_D_TERM_FILTER + dt * derivative - _last_derivative float AC_PID::get_pi float error float dt float AC_PID::get_pid float error float dt void AC_PID::reset_I void AC_PID::load_gains void AC_PID::save_gains "
ArduPilot 17 AC_PID.cpp Non-Control f1c3f2a3d1 AC_PID: Remove get_leaky_i function which is now found in AC_HELI_PID. ""
ArduPilot 18 AC_PID.cpp Control b35ec4339e AC_PID: Add method to set the D-term Filter Rate from main code. " derivative = _last_derivative + _d_lpf_alpha * derivative - _last_derivative void AC_PID::set_d_lpf_alpha int16_t cutoff_frequency float time_step float rc = 1/ 2*PI*cutoff_frequency _d_lpf_alpha = time_step / time_step + rc "
ArduPilot 19 AC_PID.cpp Control 517448e536 AC_PID: add input filtering and restructure " AP_GROUPINFO IMAX 5 AC_PID _imax 0 AP_GROUPINFO FILT_HZ 6 AC_PID _filt_hz AC_PID_FILT_HZ_DEFAULT AC_PID::AC_PID float initial_p float initial_i float initial_d float initial_imax float initial_filt_hz float dt : _dt dt _integrator 0.0f _input 0.0f _derivative 0.0f AP_Param::setup_object_defaults this var_info _kp = initial_p _ki = initial_i _kd = initial_d _imax = fabs initial_imax _filt_hz = initial_filt_hz _flags._reset_filter = true calc_filt_alpha void AC_PID::set_dt float dt _dt = dt calc_filt_alpha void AC_PID::set_filt_hz float hz _filt_hz.set hz calc_filt_alpha void AC_PID::set_input_filter_all float input if _flags._reset_filter _flags._reset_filter = false _input = input _derivative = 0.0f float input_filt_change = _filt_alpha * input - _input _input = _input + input_filt_change if _dt > 0.0f _derivative = input_filt_change / _dt void AC_PID::set_input_filter_d float input if _flags._reset_filter _flags._reset_filter = false _derivative = 0.0f if _dt > 0.0f float derivative = input - _input / _dt _derivative = _derivative + _filt_alpha * derivative-_derivative _input = input float AC_PID::get_p const return _input * _kp float AC_PID::get_i if _ki != 0 && _dt != 0 _integrator += float _input * _ki * _dt float AC_PID::get_d const return _kd * _derivative float AC_PID::get_pi return get_p + get_i float AC_PID::get_pid return get_p + get_i + get_d _imax = fabs _imax _filt_hz.load calc_filt_alpha _filt_hz.save void AC_PID::operator float p float i float d float imaxval float input_filt_hz float dt _kp = p _ki = i _kd = d _imax = fabs imaxval _filt_hz = input_filt_hz _dt = dt calc_filt_alpha void AC_PID::calc_filt_alpha float rc = 1/ 2*PI*_filt_hz _filt_alpha = _dt / _dt + rc "
ArduPilot 20 AC_PID.cpp Non-Control c10b0b34ca AC_PID: replace set_filt_hz method with filt_hzThanks to Jonathan Challinger for spotting this bugAlso add sanity check to filt_hz " #include filt_hz initial_filt_hz void AC_PID::filt_hz float hz _filt_hz.set fabs hz _filt_hz = max _filt_hz AC_PID_FILT_HZ_MIN "
ArduPilot 21 AC_PID.cpp Non-Control 12724e9556 AC_PID: remove include of stdio.h ""
ArduPilot 22 AC_PID.cpp Control ae77c18a19 AC_PID: Protect against NaN and INF " if isfinite input if _flags._reset_filter _flags._reset_filter = false _input = input _derivative = 0.0f float input_filt_change = _filt_alpha * input - _input _input = _input + input_filt_change if _dt > 0.0f _derivative = input_filt_change / _dt "
ArduPilot 23 AC_PID.cpp Control a095a8c3a1 AC_PID: more protection against NaN and Inf " if !isfinite input return if _flags._reset_filter _flags._reset_filter = false _input = input _derivative = 0.0f float input_filt_change = _filt_alpha * input - _input _input = _input + input_filt_change if _dt > 0.0f _derivative = input_filt_change / _dt if !isfinite input return "
ArduPilot 24 AC_PID.cpp Control 6ea91d012e AC_PID: calculate filt_alpha every time the filter is run " float input_filt_change = get_filt_alpha * input - _input _derivative = _derivative + get_filt_alpha * derivative-_derivative float AC_PID::get_filt_alpha const if _filt_hz == 0.0f return 1.0f return _dt / _dt + rc "
ArduPilot 25 AC_PID.cpp Control 303cfd683a AC_PID: replace floating point == with is_equalAlso add div-by-zero check to fix calc_filt_alphaAlso get_i and get_shrink_i do not need to set values to zero in Vector2fconstructor because this is already done. " if !is_equal _ki 0.0f && !is_equal _dt 0.0f if is_equal _filt_hz 0.0f float rc = 1/ M_2PI_F*_filt_hz "
ArduPilot 26 AC_PID.cpp Control 67b0c6f5c4 AC_PID: replace is_equal with is_zero " if !is_zero _ki && !is_zero _dt if is_zero _filt_hz "
ArduPilot 27 AC_PID.cpp Control 01180d52c4 AP_PID: compiler warnings: apply is_zero(float) " if !AP_Math::is_zero _ki && !AP_Math::is_zero _dt if AP_Math::is_zero _filt_hz "
ArduPilot 28 AC_PID.cpp Control 33555b7f12 AC_PID: revert AP_Math class change " if !is_zero _ki && !is_zero _dt if is_zero _filt_hz "
ArduPilot 29 AC_PID.cpp Control 9d74f57ed3 AP_PID: replace fabs() with fabsf() " _imax = fabsf initial_imax _filt_hz.set fabsf hz _imax = fabsf _imax _imax = fabsf imaxval "
ArduPilot 30 AC_PID.cpp Non-Control 967071ca62 AC_PID: Integrate PID Logging functionality. " memset &_pid_info 0 sizeof _pid_info float AC_PID::get_p _pid_info.P = _input * _kp return _pid_info.P _pid_info.I = _integrator float AC_PID::get_d _pid_info.D = _kd * _derivative return _pid_info.D "
ArduPilot 31 AC_PID.cpp Non-Control 286d084d83 AC_PID: standardize inclusion of libaries headersThis commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to so the notation #include is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source then we use the notation #include <> with the path relative to libraries folder.Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time. " #include "
ArduPilot 32 AC_PID.cpp Non-Control 831d8acca5 Remove use of PROGMEMNow variables don t have to be declared with PROGMEM anymore so removethem. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i s/ PROGMEM / /g git grep -l -z PROGMEM | xargs -0 sed -i s/PROGMEM//g The 2 commands were done so we don t leave behind spurious spaces.AVR-specific places were not changed. " const AP_Param::GroupInfo AC_PID::var_info = "
ArduPilot 33 AC_PID.cpp Non-Control 2591261af6 Global: rename min and max macros to uppercaseThe problem with using min() and max() is that they conflict with someC++ headers. Name the macros in uppercase instead. We may go case bycase later converting them to be typesafe.Changes generated with:git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)max(/\1MAX(/g git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)min(/\1MIN(/g " _filt_hz = MAX _filt_hz AC_PID_FILT_HZ_MIN "
ArduPilot 34 AC_PID.cpp Non-Control 5148e41c1a AP_Math: Cleaned macro definitionsMoved Definitions into a separate header. Replaced PI with M_PI andremoved the M_PI_*_F macros. " float rc = 1/ M_2PI*_filt_hz "
ArduPilot 35 AC_PID.cpp Non-Control 920425567c AC_PID: shorten FILT_HZ to FILT " AP_GROUPINFO FILT 6 AC_PID _filt_hz AC_PID_FILT_HZ_DEFAULT "
ArduPilot 36 AC_PID.cpp Non-Control 227e4f86d7 AC_PID: fix parameter description ""
ArduPilot 37 AC_PID.cpp Non-Control 152edf7189 Global: remove mode line from headersUsing a global .dir-locals.el file is a better alternative thanreincluding the same emacs header in every file of the project. ""
ArduPilot 38 AC_PID.cpp Control b97bf5d15e AC_PID: added feed-forward to AC_PID " AP_GROUPINFO FF 7 AC_PID _ff 0 float AC_PID::get_ff float requested_rate _pid_info.FF = float requested_rate * _ff return _pid_info.FF "
ArduPilot 40 AC_PosControl.cpp Control dcac124105 AC_PosControl: add throttle controller " AP_GROUPINFO THR_HOVER 0 AC_PosControl _throttle_hover POSCONTROL_THROTTLE_HOVER AC_PosControl::AC_PosControl const AP_AHRS& ahrs const AP_InertialNav& inav const AP_Motors& motors AC_AttitudeControl& attitude_control APM_PI& pi_alt_pos AC_PID& pid_alt_rate AC_PID& pid_alt_accel APM_PI& pi_pos_lat APM_PI& pi_pos_lon AC_PID& pid_rate_lat AC_PID& pid_rate_lon : _inav inav _motors motors _pi_alt_pos pi_alt_pos _pi_pos_lat pi_pos_lat _pi_pos_lon pi_pos_lon _dt POSCONTROL_DT_10HZ _last_update_ms 0 _last_update_rate_ms 0 _last_update_accel_ms 0 _step 0 _speed_cms POSCONTROL_SPEED _vel_z_min POSCONTROL_VEL_Z_MIN _vel_z_max POSCONTROL_VEL_Z_MAX _accel_cms POSCONTROL_ACCEL_XY_MAX _desired_roll 0.0 _desired_pitch 0.0 _leash POSCONTROL_LEASH_LENGTH_MIN float AC_PosControl::get_stopping_point_z const Vector3f& curr_pos = _inav.get_position const Vector3f& curr_vel = _inav.get_velocity float target_alt float linear_distance float linear_velocity linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_pi_alt_pos.kP if fabs curr_vel.z < linear_velocity target_alt = curr_pos.z + curr_vel.z/_pi_alt_pos.kP linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/ 2.0f*_pi_alt_pos.kP *_pi_alt_pos.kP if curr_vel.z > 0 target_alt = curr_pos.z + linear_distance + curr_vel.z*curr_vel.z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX target_alt = curr_pos.z - linear_distance + curr_vel.z*curr_vel.z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX return constrain_float target_alt curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX void AC_PosControl::fly_to_z const float alt_cm pos_to_rate_z alt_cm void AC_PosControl::climb_at_rate const float rate_target_cms const Vector3f& curr_pos = _inav.get_position _limit.pos_up = false _limit.pos_down = false if rate_target_cms<0 && !_motors.limit.throttle_lower || rate_target_cms>0 && !_motors.limit.throttle_upper _pos_target.z += rate_target_cms * _dt if _pos_target.z < curr_pos.z - POSCONTROL_LEASH_Z _pos_target.z = curr_pos.z - POSCONTROL_LEASH_Z _limit.pos_down = true if _pos_target.z > curr_pos.z + POSCONTROL_LEASH_Z _pos_target.z = curr_pos.z + POSCONTROL_LEASH_Z _limit.pos_up = true if _alt_max > 0 && _pos_target.z > _alt_max _pos_target.z = _alt_max _limit.pos_up = true pos_to_rate_z _pos_target.z void AC_PosControl::pos_to_rate_z float alt_cm const Vector3f& curr_pos = _inav.get_position float linear_distance _pos_error.z = alt_cm - curr_pos.z if _pi_alt_pos.kP != 0 linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/ 2.0f*_pi_alt_pos.kP *_pi_alt_pos.kP if _pos_error.z > 2*linear_distance _vel_target.z = safe_sqrt 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX* _pos_error.z-linear_distance else if _pos_error.z < -2.0f*linear_distance _vel_target.z = -safe_sqrt 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX* -_pos_error.z-linear_distance _vel_target.z = _pi_alt_pos.get_p _pos_error.z _vel_target.z = 0 rate_to_accel_z _vel_target.z void AC_PosControl::rate_to_accel_z float vel_target_z uint32_t now = hal.scheduler->millis const Vector3f& curr_vel = _inav.get_velocity float z_target_speed_delta float p float desired_accel _limit.vel_up = false _limit.vel_down = false if _vel_target.z < _vel_z_min _vel_target.z = _vel_z_min _limit.vel_down = true if _vel_target.z > _vel_z_max _vel_target.z = _vel_z_max _limit.vel_up = true if now - _last_update_rate_ms > 100 _vel_error.z = 0 _vel_target_filt_z = vel_target_z desired_accel = 0 else _vel_error.z = _vel_error.z + 0.20085f * vel_target_z - curr_vel.z - _vel_error.z z_target_speed_delta = 0.20085f * vel_target_z - _vel_target_filt_z _vel_target_filt_z = _vel_target_filt_z + z_target_speed_delta desired_accel = z_target_speed_delta / _dt _last_update_rate_ms = now p = _pid_alt_rate.kP * _vel_error.z desired_accel += p desired_accel = constrain_int32 desired_accel -32000 32000 accel_to_throttle desired_accel void AC_PosControl::accel_to_throttle float accel_target_z uint32_t now = hal.scheduler->millis float z_accel_meas int32_t p i d z_accel_meas = - _ahrs.get_accel_ef .z + GRAVITY_MSS * 100.0f if now - _last_update_accel_ms > 100 _accel_error.z = 0 else _accel_error.z = _accel_error.z + 0.11164f * constrain_float accel_target_z - z_accel_meas -32000 32000 - _accel_error.z _last_update_accel_ms = now p = _pid_alt_accel.get_p _accel_error.z i = _pid_alt_accel.get_integrator if !_motors.limit.throttle_lower && !_motors.limit.throttle_upper || i>0&&_accel_error.z<0 || i<0&&_accel_error.z>0 i = _pid_alt_accel.get_i _accel_error.z _dt d = _pid_alt_accel.get_d _accel_error.z _dt _attitude_control.set_throttle_out int16_t p+i+d+_throttle_hover true "
ArduPilot 41 AC_PosControl.cpp Control 8988b48ad8 AC_PosControl: add init take-off " void AC_PosControl::set_target_to_stopping_point_z _pos_target.z = curr_pos.z + curr_vel.z/_pi_alt_pos.kP _pos_target.z = curr_pos.z + linear_distance + curr_vel.z*curr_vel.z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX _pos_target.z = curr_pos.z - linear_distance + curr_vel.z*curr_vel.z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX _pos_target.z = constrain_float _pos_target.z curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX void AC_PosControl::init_takeoff const Vector3f& curr_pos = _inav.get_position _pos_target.z = curr_pos.z + POSCONTROL_TAKEOFF_JUMP_CM if _pid_alt_accel.get_integrator < 0 _pid_alt_accel.reset_I void AC_PosControl::fly_to_target_z pos_to_rate_z pos_to_rate_z void AC_PosControl::pos_to_rate_z _pos_error.z = _pos_target.z - curr_pos.z "
ArduPilot 42 AC_PosControl.cpp Non-Control 3ce1c0a9d5 AC_PosControl: add get_alt_error method " float AC_PosControl::get_alt_error const return _pos_target.z - _inav.get_position .z */"
ArduPilot 43 AC_PosControl.cpp Control bbcf8cc84c AC_PosControl: add horizontal pos control " _speed_down_cms POSCONTROL_SPEED_DOWN _speed_up_cms POSCONTROL_SPEED_UP _accel_z_cms POSCONTROL_ACCEL_XY_MAX _leash POSCONTROL_LEASH_LENGTH_MIN _roll_target 0.0 _pitch_target 0.0 _vel_target_filt_z 0 _alt_max 0 _distance_to_target 0 _xy_step 0 _dt_xy 0 _flags.force_recalc_xy = false #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 _flags.slow_cpu = false #else _flags.slow_cpu = true #endif void AC_PosControl::update_z_controller float linear_distance if _vel_target.z < _speed_down_cms _vel_target.z = _speed_down_cms if _vel_target.z > _speed_up_cms _vel_target.z = _speed_up_cms void AC_PosControl::set_pos_target const Vector3f& position _pos_target = position _roll_target = constrain_int32 _ahrs.roll_sensor -_attitude_control.lean_angle_max _attitude_control.lean_angle_max _pitch_target = constrain_int32 _ahrs.pitch_sensor -_attitude_control.lean_angle_max _attitude_control.lean_angle_max void AC_PosControl::get_stopping_point_xy Vector3f &stopping_point const Vector3f curr_pos = _inav.get_position Vector3f curr_vel = _inav.get_velocity float linear_distance float linear_velocity float stopping_dist float kP = _pi_pos_lat.kP float vel_total = safe_sqrt curr_vel.x*curr_vel.x + curr_vel.y*curr_vel.y if vel_total < 10.0f || kP <= 0.0f || _accel_cms <= 0.0f stopping_point = curr_pos linear_velocity = _accel_cms/kP stopping_dist = vel_total/kP linear_distance = _accel_cms/ 2.0f*kP*kP stopping_dist = linear_distance + vel_total*vel_total / 2.0f*_accel_cms stopping_dist = constrain_float stopping_dist 0 _leash stopping_point.x = curr_pos.x + stopping_dist * curr_vel.x / vel_total stopping_point.y = curr_pos.y + stopping_dist * curr_vel.y / vel_total void AC_PosControl::update_pos_controller bool use_desired_velocity uint32_t now = hal.scheduler->millis if now - _last_update_ms >= 1000 _last_update_ms = now reset_I_xy _xy_step = 0 if _flags.force_recalc_xy && _xy_step > 3 _flags.force_recalc_xy = false _xy_step = 0 switch _xy_step _dt_xy = now - _last_update_ms / 1000.0f _last_update_ms = now desired_vel_to_pos _dt_xy _xy_step++ hal.console->printf_P PSTR 0 pos_to_rate_xy use_desired_velocity _dt_xy _xy_step++ hal.console->printf_P PSTR 1 rate_to_accel_xy _dt_xy _xy_step++ hal.console->printf_P PSTR 2 accel_to_lean_angles _xy_step++ hal.console->printf_P PSTR 3 void AC_PosControl::desired_vel_to_pos float nav_dt Vector2f target_vel_adj float vel_desired_total if nav_dt < 0 vel_desired_total = safe_sqrt _vel_desired.x*_vel_desired.x + _vel_desired.y*_vel_desired.y if vel_desired_total > _speed_cms && vel_desired_total > 0.0f _vel_desired.x = _speed_cms * _vel_desired.x/vel_desired_total _vel_desired.y = _speed_cms * _vel_desired.y/vel_desired_total _pos_target.x += _vel_desired.x * nav_dt _pos_target.y += _vel_desired.y * nav_dt void AC_PosControl::pos_to_rate_xy bool use_desired_rate float dt Vector3f curr_pos = _inav.get_position float linear_distance float kP = _pi_pos_lat.kP _vel_target.x = 0.0 _vel_target.y = 0.0 _pos_error.x = _pos_target.x - curr_pos.x _pos_error.y = _pos_target.y - curr_pos.y _distance_to_target = safe_sqrt _pos_error.x*_pos_error.x + _pos_error.y*_pos_error.y if _distance_to_target > _leash && _distance_to_target > 0.0f _pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target _pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target _pos_error.x = _pos_target.x - curr_pos.x _pos_error.y = _pos_target.y - curr_pos.y _distance_to_target = _leash linear_distance = _accel_cms/ 2.0f*kP*kP if _distance_to_target > 2.0f*linear_distance float vel_sqrt = safe_sqrt 2.0f*_accel_cms* _distance_to_target-linear_distance _vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target _vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target else _vel_target.x = _pi_pos_lat.kP * _pos_error.x _vel_target.y = _pi_pos_lon.kP * _pos_error.y float vel_max_from_pos_error if use_desired_rate vel_max_from_pos_error = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR vel_max_from_pos_error = _speed_cms float vel_total = safe_sqrt _vel_target.x*_vel_target.x + _vel_target.y*_vel_target.y if vel_total > vel_max_from_pos_error _vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total _vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total if use_desired_rate _vel_target.x += _vel_desired.x _vel_target.y += _vel_desired.y void AC_PosControl::rate_to_accel_xy float dt const Vector3f &vel_curr = _inav.get_velocity _limit.accel_xy = false if dt == 0.0 _accel_target.x = 0 _accel_target.y = 0 _accel_target.x = _vel_target.x - _vel_last.x /dt _accel_target.y = _vel_target.y - _vel_last.y /dt _vel_last.x = _vel_target.x _vel_last.y = _vel_target.y _vel_error.x = _vel_target.x - vel_curr.x _vel_error.y = _vel_target.y - vel_curr.y _accel_target.x += _pid_rate_lat.get_pid _vel_error.x dt _accel_target.y += _pid_rate_lon.get_pid _vel_error.y dt accel_total = safe_sqrt _accel_target.x*_accel_target.x + _accel_target.y*_accel_target.y if accel_total > POSCONTROL_ACCEL_XY_MAX _accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total _accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total _limit.accel_xy = true void AC_PosControl::accel_to_lean_angles float accel_right accel_forward float lean_angle_max = _attitude_control.lean_angle_max accel_forward = _accel_target.x*_cos_yaw + _accel_target.y*_sin_yaw accel_right = -_accel_target.x*_sin_yaw + _accel_target.y*_cos_yaw _roll_target = constrain_float fast_atan accel_right*_cos_pitch/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max void AC_PosControl::reset_I_xy _pi_pos_lon.reset_I _pi_pos_lat.reset_I _pid_rate_lon.reset_I _pid_rate_lat.reset_I _vel_last = _inav.get_velocity float AC_PosControl::calc_leash_length float speed_cms float accel_cms float kP const float leash_length if accel_cms <= 0.0f accel_cms = POSCONTROL_ACCELERATION_MIN if kP <= 0.0f return POSCONTROL_LEASH_LENGTH_MIN if speed_cms <= accel_cms / kP leash_length = speed_cms / kP else leash_length = accel_cms / 2.0f*kP*kP + speed_cms*speed_cms / 2.0f*accel_cms if leash_length < POSCONTROL_LEASH_LENGTH_MIN leash_length = POSCONTROL_LEASH_LENGTH_MIN return leash_length "
ArduPilot 44 AC_PosControl.cpp Non-Control 80ae3dca2e AC_PosControl: remove debug " _pitch_target = constrain_float fast_atan -accel_forward/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max "
ArduPilot 45 AC_PosControl.cpp Control de34359808 AC_PosControl: recalculate leash when speed or accel modified " _flags.recalc_leash_xy = true _flags.recalc_leash_z = true void AC_PosControl::set_speed_z float speed_down float speed_up if fabs _speed_down_cms-speed_down > 1.0f || fabs _speed_up_cms-speed_up > 1.0f _speed_down_cms = speed_down _speed_up_cms = speed_up _flags.recalc_leash_z = true void AC_PosControl::set_accel_z float accel_cmss if fabs _accel_z_cms-accel_cmss > 1.0f _accel_z_cms = accel_cmss _flags.recalc_leash_z = true void AC_PosControl::set_alt_target_with_slew float alt_cm float dt float alt_change = alt_cm-_pos_target.z if alt_change<0 && !_motors.limit.throttle_lower || alt_change>0 && !_motors.limit.throttle_upper _pos_target.z += constrain_float alt_change _speed_down_cms*dt _speed_up_cms*dt float curr_alt = _inav.get_altitude _pos_target.z = constrain_float _pos_target.z curr_alt-_leash_down_z curr_alt+_leash_up_z void AC_PosControl::set_alt_target_from_climb_rate float climb_rate_cms float dt if climb_rate_cms<0 && !_motors.limit.throttle_lower || climb_rate_cms>0 && !_motors.limit.throttle_upper _pos_target.z += climb_rate_cms * _dt return _pos_target.z - _inav.get_altitude calc_leash_length_z void AC_PosControl::calc_leash_length_z if _flags.recalc_leash_z _leash_up_z = calc_leash_length _speed_up_cms _accel_z_cms _pi_alt_pos.kP _leash_down_z = calc_leash_length _speed_down_cms _accel_z_cms _pi_alt_pos.kP _flags.recalc_leash_z = false hal.console->printf_P PSTR \nLLZ:%4.2f %4.2f\n float _leash_up_z float _leash_down_z void AC_PosControl::pos_to_rate_z float curr_alt = _inav.get_altitude float linear_distance _pos_error.z = _pos_target.z - curr_alt if _pos_error.z > _leash_up_z _pos_target.z = curr_alt + _leash_up_z if _pos_error.z < -_leash_down_z _pos_target.z = curr_alt - _leash_down_z _limit.pos_down = true void AC_PosControl::set_accel_xy float accel_cmss if fabs _accel_cms-accel_cmss > 1.0f _accel_cms = accel_cmss _flags.recalc_leash_xy = true void AC_PosControl::set_speed_xy float speed_cms if fabs _speed_cms-speed_cms > 1.0f _speed_cms = speed_cms _flags.recalc_leash_xy = true static uint8_t counter = 0 counter++ if counter >= 10 counter = 0 hal.console->printf_P PSTR \nPosX:%4.2f Y:%4.2f Z:%4.2f\n float _pos_target.x float _pos_target.y float _pos_target.z calc_leash_length_xy void AC_PosControl::calc_leash_length_xy if _flags.recalc_leash_xy _leash = calc_leash_length _speed_cms _accel_cms _pi_pos_lon.kP _flags.recalc_leash_xy = false hal.console->printf_P PSTR \nXYA:%4.2f S:%4.2f kP:%4.2f\n float _accel_cms float _speed_cms float _pi_pos_lon.kP hal.console->printf_P PSTR \nLLXY:%4.2f\n float _leash "
ArduPilot 46 AC_PosControl.cpp Non-Control 1b8791a142 AC_PosControl: remove debug ""
ArduPilot 47 AC_PosControl.cpp Control 26b257c8ba AC_AttControl: alternative get_stopping_point_z " get_stopping_point_z _pos_target void AC_PosControl::get_stopping_point_z Vector3f& stopping_point const float curr_pos_z = _inav.get_altitude const float curr_vel_z = _inav.get_velocity_z if fabs curr_vel_z < linear_velocity stopping_point.z = curr_pos_z + curr_vel_z/_pi_alt_pos.kP if curr_vel_z > 0 stopping_point.z = curr_pos_z + linear_distance + curr_vel_z*curr_vel_z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX stopping_point.z = curr_pos_z - linear_distance + curr_vel_z*curr_vel_z/ 2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX stopping_point.z = constrain_float stopping_point.z curr_pos_z - POSCONTROL_STOPPING_DIST_Z_MAX curr_pos_z + POSCONTROL_STOPPING_DIST_Z_MAX "
ArduPilot 48 AC_PosControl.cpp Control 551836c49d AC_PosControl: make some methods const " void AC_PosControl::get_stopping_point_z Vector3f& stopping_point const"
ArduPilot 49 AC_PosControl.cpp Control 8b8d6a8e01 AC_PosControl: use trig values from ahrs " accel_forward = _accel_target.x*_ahrs.cos_yaw + _accel_target.y*_ahrs.sin_yaw accel_right = -_accel_target.x*_ahrs.sin_yaw + _accel_target.y*_ahrs.cos_yaw _roll_target = constrain_float fast_atan accel_right*_ahrs.cos_pitch / GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max "
ArduPilot 50 AC_PosControl.cpp Control 94fb9c4274 AC_PosControl: replace APM_PI with AC_P " AC_P& p_alt_pos AC_PID& pid_alt_rate AC_PID& pid_alt_accel AC_P& p_pos_xy AC_PID& pid_rate_lat AC_PID& pid_rate_lon : _p_alt_pos p_alt_pos _p_pos_xy p_pos_xy linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_p_alt_pos.kP stopping_point.z = curr_pos_z + curr_vel_z/_p_alt_pos.kP linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/ 2.0f*_p_alt_pos.kP *_p_alt_pos.kP _leash_up_z = calc_leash_length _speed_up_cms _accel_z_cms _p_alt_pos.kP _leash_down_z = calc_leash_length _speed_down_cms _accel_z_cms _p_alt_pos.kP if _p_alt_pos.kP != 0 linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/ 2.0f*_p_alt_pos.kP *_p_alt_pos.kP _vel_target.z = _p_alt_pos.get_p _pos_error.z float kP = _p_pos_xy.kP _leash = calc_leash_length _speed_cms _accel_cms _p_pos_xy.kP float kP = _p_pos_xy.kP _vel_target.x = _p_pos_xy.kP * _pos_error.x _vel_target.y = _p_pos_xy.kP * _pos_error.y "
ArduPilot 51 AC_PosControl.cpp Control 9130c88f15 AC_PosControl: throttle rate to simple P controller " AC_P& p_alt_pos AC_P& p_alt_rate AC_PID& pid_alt_accel _p_alt_rate p_alt_rate p = _p_alt_rate.kP * _vel_error.z "
ArduPilot 52 AC_PosControl.cpp Control 024855014f AC_PosControl: update _pos_error if it is being limitedThis fixes a bug that causes 10hz throttle noise. " _pos_error.z = _leash_up_z _pos_error.z = -_leash_down_z "
ArduPilot 53 AC_PosControl.cpp Control 345115fddd AC_PosControl: fixed leash length calc for descent speed " _leash_down_z = calc_leash_length -_speed_down_cms _accel_z_cms _p_alt_pos.kP "
ArduPilot 54 AC_PosControl.cpp Non-Control dd45647626 AC_PosControl: update some comments re leash lengths " float linear_distance "
ArduPilot 55 AC_PosControl.cpp Control 90e205c905 AC_PosControl: replace safe_sqrt with pythagorous2 " float vel_total = pythagorous2 curr_vel.x curr_vel.y vel_desired_total = pythagorous2 _vel_desired.x _vel_desired.y _distance_to_target = pythagorous2 _pos_error.x _pos_error.y float vel_total = pythagorous2 _vel_target.x _vel_target.y accel_total = pythagorous2 _accel_target.x _accel_target.y "
ArduPilot 56 AC_PosControl.cpp Non-Control 7e376bc517 PosControl: update_pos_controller renamed to update_xy_controller " void AC_PosControl::update_xy_controller bool use_desired_velocity "
ArduPilot 57 AC_PosControl.cpp Control 6f6c9e2585 AC_PosControl: bug fix to vertical speed limitVehicle was not reaching target climb or descent rate because ofincorrectly defaulted acceleration " _accel_z_cms POSCONTROL_ACCEL_Z _accel_cms POSCONTROL_ACCEL_XY linear_velocity = _accel_z_cms/_p_alt_pos.kP linear_distance = _accel_z_cms/ 2.0f*_p_alt_pos.kP *_p_alt_pos.kP stopping_point.z = curr_pos_z + linear_distance + curr_vel_z*curr_vel_z/ 2.0f*_accel_z_cms stopping_point.z = curr_pos_z - linear_distance + curr_vel_z*curr_vel_z/ 2.0f*_accel_z_cms linear_distance = _accel_z_cms/ 2.0f*_p_alt_pos.kP *_p_alt_pos.kP _vel_target.z = safe_sqrt 2.0f*_accel_z_cms* _pos_error.z-linear_distance _vel_target.z = -safe_sqrt 2.0f*_accel_z_cms* -_pos_error.z-linear_distance "
ArduPilot 58 AC_PosControl.cpp Control e565ee6d33 AC_PosControl: stop I term build-up when motors at max " float lat_i lon_i lat_i = _pid_rate_lat.get_integrator lon_i = _pid_rate_lon.get_integrator if !_limit.accel_xy && !_motors.limit.throttle_upper || lat_i>0&&_vel_error.x<0 || lat_i<0&&_vel_error.x>0 lat_i = _pid_rate_lat.get_i _vel_error.x dt if !_limit.accel_xy && !_motors.limit.throttle_upper || lon_i>0&&_vel_error.y<0 || lon_i<0&&_vel_error.y>0 lon_i = _pid_rate_lat.get_i _vel_error.y dt _accel_target.x += _pid_rate_lat.get_p _vel_error.x + lat_i + _pid_rate_lat.get_d _vel_error.x dt _accel_target.y += _pid_rate_lon.get_p _vel_error.y + lon_i + _pid_rate_lon.get_d _vel_error.y dt else _limit.accel_xy = false "
ArduPilot 59 AC_PosControl.cpp Control 09a35cf90f AC_PosControl: bugfix for freezing I-term build-up " lon_i = _pid_rate_lon.get_i _vel_error.y dt "
ArduPilot 60 AC_PosControl.cpp Control 6c71569775 AC_PosControl: update_xy_controller allows not resetting I term " void AC_PosControl::update_xy_controller bool use_desired_velocity bool reset_I if reset_I reset_I_xy "
ArduPilot 61 AC_PosControl.cpp Control 098f8169b0 AC_PosControl: add keep_xy_I_terms methodStops horizontal PID s I terms from being reset when the controller isnext updated " _flags.keep_xy_I_terms = false void AC_PosControl::update_xy_controller bool use_desired_velocity if !_flags.keep_xy_I_terms _flags.keep_xy_I_terms = false "
ArduPilot 62 AC_PosControl.cpp Control 9f63de9b59 AC_PosControl: set_speed_z accepts positive descent speeds " speed_down = -fabs speed_down "
ArduPilot 63 AC_PosControl.cpp Control 937e9ea687 AC_PosControl: add set_target_to_stopping_point_xy methodFixed bug in get_stopping_point_xy in which it would update Z-axistarget if vehicle was moving less than 10cm/s horizontally " calc_leash_length_z void AC_PosControl::set_target_to_stopping_point_xy calc_leash_length_xy get_stopping_point_xy _pos_target stopping_point.x = curr_pos.x stopping_point.y = curr_pos.y "
ArduPilot 64 AC_PosControl.cpp Control a1f1dd8059 AC_PosControl: add is_active_z methodConsolidated z-axis timeout checks to save 4bytes of RAMAdded POS_CONTROL_ACTIVE_TIMEOUT_MS to make timeout consistent " _last_update_xy_ms 0 _last_update_z_ms 0 _flags.reset_rate_to_accel_z = true _flags.reset_accel_to_throttle = true bool AC_PosControl::is_active_z const return hal.scheduler->millis - _last_update_z_ms <= POSCONTROL_ACTIVE_TIMEOUT_MS uint32_t now = hal.scheduler->millis if now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS _flags.reset_rate_to_accel_z = true _flags.reset_accel_to_throttle = true _last_update_z_ms = now if _flags.reset_rate_to_accel_z _flags.reset_rate_to_accel_z = false if _flags.reset_accel_to_throttle _flags.reset_accel_to_throttle = false if now - _last_update_xy_ms >= POSCONTROL_ACTIVE_TIMEOUT_MS _last_update_xy_ms = now _dt_xy = now - _last_update_xy_ms / 1000.0f _last_update_xy_ms = now "
ArduPilot 65 AC_PosControl.cpp Control 21c93e48ab AC_PosControl: include vel error when get_stopping_point_zPair programmed with Randy " float curr_vel_z = _inav.get_velocity_z if is_active_z curr_vel_z += _vel_error.z "
ArduPilot 66 AC_PosControl.cpp Control 0819e05896 AC_PosControl: lean_angles_to_accel added for smooth initialisationinit_xy_controller also added to capture initialisation all in one place " _flags.reset_desired_vel_to_pos = true _flags.reset_rate_to_accel_xy = true void AC_PosControl::set_xy_target float x float y _pos_target.x = x _pos_target.y = y if is_active_xy curr_vel.x += _vel_error.x curr_vel.y += _vel_error.y if kP <= 0.0f || _accel_cms <= 0.0f bool AC_PosControl::is_active_xy const return hal.scheduler->millis - _last_update_xy_ms <= POSCONTROL_ACTIVE_TIMEOUT_MS void AC_PosControl::init_xy_controller _xy_step = 0 _roll_target = _ahrs.roll_sensor _pitch_target = _ahrs.pitch_sensor if !_flags.keep_xy_I_terms reset_I_xy else _flags.keep_xy_I_terms = false lean_angles_to_accel _accel_target.x _accel_target.y _pid_rate_lat.set_integrator _accel_target.x _pid_rate_lon.set_integrator _accel_target.y _flags.reset_desired_vel_to_pos = true _flags.reset_rate_to_accel_xy = true _last_update_xy_ms = hal.scheduler->millis init_xy_controller if _flags.reset_desired_vel_to_pos _flags.reset_desired_vel_to_pos = false else _pos_target.x += _vel_desired.x * nav_dt _pos_target.y += _vel_desired.y * nav_dt if _flags.reset_rate_to_accel_xy _vel_last.x = _vel_target.x _vel_last.y = _vel_target.y _flags.reset_rate_to_accel_xy = false if dt > 0.0f else _accel_target.x = 0.0f _accel_target.y = 0.0f if accel_total > POSCONTROL_ACCEL_XY_MAX && accel_total > 0.0f void AC_PosControl::lean_angles_to_accel float& accel_x_cmss float& accel_y_cmss accel_x_cmss = GRAVITY_MSS * 100 * - _ahrs.cos_yaw * _ahrs.sin_pitch / max _ahrs.cos_pitch 0.5 - _ahrs.sin_yaw * _ahrs.sin_roll / max _ahrs.cos_roll 0.5 accel_y_cmss = GRAVITY_MSS * 100 * - _ahrs.sin_yaw * _ahrs.sin_pitch / max _ahrs.cos_pitch 0.5 + _ahrs.cos_yaw * _ahrs.sin_roll / max _ahrs.cos_roll 0.5 "
ArduPilot 67 AC_PosControl.cpp Control cd8b1f278c AC_PosControl: remove unused _cos_yaw variableAlso removed _sin_yaw _cos_pitch _step saving a total of 17bytes ofRAMAlso made get_speed_up get_speed_down lean_angles_to_accel functionsconst " const Vector3f curr_pos = _inav.get_position void AC_PosControl::lean_angles_to_accel float& accel_x_cmss float& accel_y_cmss const"
ArduPilot 68 AC_PosControl.cpp Control c13db680b7 AC_PosControl: remove reset_I_xylean_angles_to_accel is now used to load the position rate I terms basedon vehicle s initial roll and pitch angle " lean_angles_to_accel _accel_target.x _accel_target.y _pid_rate_lat.set_integrator _accel_target.x _pid_rate_lon.set_integrator _accel_target.y "
ArduPilot 69 AC_PosControl.cpp Control 29ca7a10df AC_PosControl: set alt hold accel control D term filter " void AC_PosControl::set_dt float delta_sec _dt = delta_sec _pid_alt_accel.set_d_lpf_alpha POSCONTROOL_ACCEL_Z_DTERM_FILTER _dt "
ArduPilot 70 AC_PosControl.cpp Non-Control 2b0f142a17 AC_PosControl: fix typo in D-filter definition " _pid_alt_accel.set_d_lpf_alpha POSCONTROL_ACCEL_Z_DTERM_FILTER _dt "
ArduPilot 71 AC_PosControl.cpp Control 0d87298221 AC_PosControl: freeze feed forward and vector fixes " rate_to_accel_z void AC_PosControl::rate_to_accel_z if _flags.reset_rate_to_accel_z _vel_last.z = _vel_target.z _flags.reset_rate_to_accel_z = false if _dt > 0.0f if !_limit.freeze_ff_z _accel_feedforward.z = _vel_target.z - _vel_last.z /_dt else _limit.freeze_ff_z = false else _accel_feedforward.z = 0.0f _vel_last.z = _vel_target.z _vel_error.z = _vel_target.z - curr_vel.z desired_accel = _accel_feedforward.z + p if !_limit.freeze_ff_xy _accel_feedforward.x = _vel_target.x - _vel_last.x /dt _accel_feedforward.y = _vel_target.y - _vel_last.y /dt else _limit.freeze_ff_xy = false _accel_feedforward.x = 0.0f _accel_feedforward.y = 0.0f _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p _vel_error.x + lat_i + _pid_rate_lat.get_d _vel_error.x dt _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p _vel_error.y + lon_i + _pid_rate_lon.get_d _vel_error.y dt "
ArduPilot 72 AC_PosControl.cpp Non-Control 8bbce7e658 AC_PosControl: freeze feed forward for alt control in Auto ""
ArduPilot 73 AC_PosControl.cpp Non-Control 4d4c7a2118 AC_AttControl: move freeze_ff to flags structure " if !_flags.freeze_ff_z _flags.freeze_ff_z = false if !_flags.freeze_ff_xy _flags.freeze_ff_xy = false "
ArduPilot 74 AC_PosControl.cpp Control 82ed70b25e AC_PosControl: add xyz velocity controllerVelocity controller interpretsthe velocity requests asdesired velocities (i.e. feed forward). These are then used to updatethe target position and also added to the target velocity.Also renamed the set_desired_velocity() function toset_desired_velocity_xy() to make clear only lat and lon axis areupdated. " _last_update_vel_xyz_ms 0 _dt_xy 0 _vel_xyz_step 0 void AC_PosControl::init_vel_controller_xyz _vel_xyz_step = 3 _roll_target = _ahrs.roll_sensor _pitch_target = _ahrs.pitch_sensor lean_angles_to_accel _accel_target.x _accel_target.y _pid_rate_lat.set_integrator _accel_target.x _pid_rate_lon.set_integrator _accel_target.y _flags.reset_desired_vel_to_pos = true _flags.reset_rate_to_accel_xy = true const Vector3f& curr_pos = _inav.get_position set_xy_target curr_pos.x curr_pos.y const Vector3f& curr_vel = _inav.get_velocity set_desired_velocity_xy curr_vel.x curr_vel.y _last_update_vel_xyz_ms = hal.scheduler->millis void AC_PosControl::update_vel_controller_xyz uint32_t now = hal.scheduler->millis float dt_xy = now - _last_update_vel_xyz_ms / 1000.0f calc_leash_length_xy if dt_xy >= POSCONTROL_VEL_UPDATE_TIME _last_update_vel_xyz_ms = now if dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS dt_xy = 0.0f desired_vel_to_pos dt_xy pos_to_rate_xy true dt_xy rate_to_accel_xy dt_xy accel_to_lean_angles set_alt_target_from_climb_rate _vel_desired.z _dt update_z_controller "
ArduPilot 75 AC_PosControl.cpp Control 5f66027ba3 AC_AttitudeControl: Fixup some minor mistakes in AC_PosControl " _roll_target 0.0f _pitch_target 0.0f _pos_target.z += climb_rate_cms * dt _vel_target.x = 0.0f _vel_target.y = 0.0f accel_x_cmss = GRAVITY_MSS * 100 * - _ahrs.cos_yaw * _ahrs.sin_pitch / max _ahrs.cos_pitch 0.5f - _ahrs.sin_yaw * _ahrs.sin_roll / max _ahrs.cos_roll 0.5f accel_y_cmss = GRAVITY_MSS * 100 * - _ahrs.sin_yaw * _ahrs.sin_pitch / max _ahrs.cos_pitch 0.5f + _ahrs.cos_yaw * _ahrs.sin_roll / max _ahrs.cos_roll 0.5f "
ArduPilot 76 AC_PosControl.cpp Control 762bb3e6e8 AC_AttitudeControl: Limit _pos_target.z to below alt_max before computing error " if _alt_max > 0 && _pos_target.z > _alt_max _pos_target.z = _alt_max _limit.pos_up = true "
ArduPilot 77 AC_PosControl.cpp Control 9233bbab14 AC_PosControl: cast fabs to float to resolve compiler warnings " speed_down = float -fabs speed_down if float fabs _speed_down_cms-speed_down > 1.0f || float fabs _speed_up_cms-speed_up > 1.0f if float fabs _accel_z_cms-accel_cmss > 1.0f if float fabs curr_vel_z < linear_velocity if float fabs _accel_cms-accel_cmss > 1.0f if float fabs _speed_cms-speed_cms > 1.0f "
ArduPilot 78 AC_PosControl.cpp Control 70568225a6 AC_PosControl: init members to resolve compiler warnings " _leash_down_z POSCONTROL_LEASH_LENGTH_MIN _leash_up_z POSCONTROL_LEASH_LENGTH_MIN _alt_max 0.0f _distance_to_target 0.0f _dt_xy 0.0f if _p_alt_pos.kP != 0.0f "
ArduPilot 79 AC_PosControl.cpp Control 1362bdc338 AC_PosControl: smooth take-off with accel PID s I termThis avoids an instantaneous jump in throttle during take-off by loadingthe accel PID s I term with the expected change in throttle level " freeze_ff_z _pid_alt_accel.set_integrator _motors.get_throttle_out -_throttle_hover "
ArduPilot 80 AC_PosControl.cpp Control 665f353416 AC_PosControl: 2hz filter on accel errorReplaced hard-coded filter with LowPassFilter class allowing thefilter s to be 2hz on both APM and Pixhawk " _accel_error_filter.set_cutoff_frequency _dt POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ _accel_error_filter.reset 0 _accel_error.z = _accel_error_filter.apply constrain_float accel_target_z - z_accel_meas -32000 32000 "
ArduPilot 81 AC_PosControl.cpp Control cf3b2be99c AC_PosControl: 4hz filter on z-axis velocity error " _vel_error_filter.set_cutoff_frequency _dt POSCONTROL_VEL_ERROR_CUTOFF_FREQ _vel_error_filter.reset 0 _vel_error.z = _vel_error_filter.apply _vel_target.z - curr_vel.z "
ArduPilot 82 AC_PosControl.cpp Non-Control 1754cacf3c AC_PosControl: remove completed to-do comments ""
ArduPilot 83 AC_PosControl.cpp Control 4a397a8d67 AC_PosControl: Protect from divide-by-zero in get_stopping_point_xy " if kP <= 0.0f || _accel_cms <= 0.0f || vel_total == 0.0f "
ArduPilot 84 AC_PosControl.cpp Control a1cfd03c9b AC_PosControl: bug fix dt calculationfixes issue in which now could be earlier than _last_update_xy_msleading to a large dt value and a sudden lean on takeoff " now = _last_update_xy_ms "
ArduPilot 85 AC_PosControl.cpp Control 779baa006d AC_PosControl: Use sqrt_controller function " _vel_target.z = sqrt_controller _pos_error.z _p_alt_pos.kP _accel_z_cms "
ArduPilot 86 AC_PosControl.cpp Control ac7ea2a12c AC_PosControl: use AttControl s sqrt_controller " _vel_target.z = AC_AttitudeControl::sqrt_controller _pos_error.z _p_alt_pos.kP _accel_z_cms "
ArduPilot 87 AC_PosControl.cpp Control e81c1dd5a1 AC_PosControl: add force_descend option to set_alt_target_from_climb_rate " void AC_PosControl::set_alt_target_from_climb_rate float climb_rate_cms float dt bool force_descend if climb_rate_cms<0 && !_motors.limit.throttle_lower || force_descend || climb_rate_cms>0 && !_motors.limit.throttle_upper "
ArduPilot 88 AC_PosControl.cpp Control bfe71fea82 AC_PosControl: Use blended accelerometer measurement in altitude control " z_accel_meas = - _ahrs.get_accel_ef_blended .z + GRAVITY_MSS * 100.0f "
ArduPilot 89 AC_PosControl.cpp Control 2dff76394d AC_PosControl: Add spike and noise filter to demanded anglesThis has been done to provide a smooth psotion hold when using an optical flow aided EKF which can be noisy.Signed-off-by: priseborough " uint32_t now = hal.scheduler->millis if now - _last_update_xy_ms >= POSCONTROL_ACTIVE_TIMEOUT_MS now = _last_update_xy_ms float dt_xy = now - _last_update_xy_ms * 0.001f static float lastRollDem = 0.0f static float lastPitchDem = 0.0f float maxDeltaAngle = dt_xy * 10000.0f if _roll_target - lastRollDem > maxDeltaAngle _roll_target = lastRollDem + maxDeltaAngle else if _roll_target - lastRollDem < -maxDeltaAngle _roll_target = lastRollDem - maxDeltaAngle lastRollDem = _roll_target if _pitch_target - lastPitchDem > maxDeltaAngle _pitch_target = lastPitchDem + maxDeltaAngle else if _pitch_target - lastPitchDem < -maxDeltaAngle _pitch_target = lastPitchDem - maxDeltaAngle lastPitchDem = _pitch_target float freq_cut = 5.0f float alpha = constrain_float dt_xy/ dt_xy + 1.0f/ 2.0f* float M_PI*freq_cut 0.0f 1.0f static float roll_target_filtered = 0.0f static float pitch_target_filtered = 0.0f roll_target_filtered += alpha * _roll_target - roll_target_filtered pitch_target_filtered += alpha * _pitch_target - pitch_target_filtered _roll_target = roll_target_filtered _pitch_target = pitch_target_filtered "
ArduPilot 90 AC_PosControl.cpp Control e80b1c67cd AC_AttitudeControl: Add EKF optical flow noise gain scalerAllows gains to be adjusted to compensate for optical flow noise " void AC_PosControl::update_xy_controller bool use_desired_velocity float ekfNavVelGainScaler pos_to_rate_xy use_desired_velocity _dt_xy ekfNavVelGainScaler rate_to_accel_xy _dt_xy ekfNavVelGainScaler void AC_PosControl::update_vel_controller_xyz float ekfNavVelGainScaler pos_to_rate_xy true dt_xy ekfNavVelGainScaler rate_to_accel_xy dt_xy ekfNavVelGainScaler void AC_PosControl::pos_to_rate_xy bool use_desired_rate float dt float ekfNavVelGainScaler float kP = ekfNavVelGainScaler * _p_pos_xy.kP void AC_PosControl::rate_to_accel_xy float dt float ekfNavVelGainScaler _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p _vel_error.x + lat_i + _pid_rate_lat.get_d _vel_error.x dt * ekfNavVelGainScaler _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p _vel_error.y + lon_i + _pid_rate_lon.get_d _vel_error.y dt * ekfNavVelGainScaler "
ArduPilot 91 AC_PosControl.cpp Control 12ea1d6e85 AC_AttitudeControl: Scale angle demand noise filterScales filter frequency using EKF nav gain scaler to take advantage of the reduced nav frequency whichcan tolerate a slower angle response.This is required to reduce the effect of EKF optical flow noise increase with height. " accel_to_lean_angles ekfNavVelGainScaler accel_to_lean_angles ekfNavVelGainScaler void AC_PosControl::accel_to_lean_angles float ekfNavVelGainScaler float freq_cut = 5.0f * ekfNavVelGainScaler "
ArduPilot 92 AC_PosControl.cpp Control d6e455417f AC_PosControl: change keep_xy_I_terms to a parameter of init_xy_controller " void AC_PosControl::init_xy_controller bool reset_I if reset_I "
ArduPilot 93 AC_PosControl.cpp Control 557d339cf1 AC_PosControl: remove various unnecessary timing hacks " _dt_xy POSCONTROL_DT_50HZ _distance_to_target 0.0f float dt = now - _last_update_xy_ms / 1000.0f _last_update_xy_ms = now if dt > POSCONTROL_ACTIVE_TIMEOUT_MS dt = 0.0f desired_vel_to_pos dt pos_to_rate_xy use_desired_velocity dt ekfNavVelGainScaler rate_to_accel_xy dt ekfNavVelGainScaler accel_to_lean_angles dt ekfNavVelGainScaler float AC_PosControl::time_since_last_xy_update const uint32_t now = hal.scheduler->millis return now - _last_update_xy_ms *0.001f float dt = now - _last_update_xy_ms / 1000.0f if dt >= POSCONTROL_ACTIVE_TIMEOUT_MS dt = 0.0f desired_vel_to_pos dt pos_to_rate_xy true dt ekfNavVelGainScaler rate_to_accel_xy dt ekfNavVelGainScaler accel_to_lean_angles dt ekfNavVelGainScaler set_alt_target_from_climb_rate _vel_desired.z dt _last_update_xy_ms = now void AC_PosControl::accel_to_lean_angles float dt float ekfNavVelGainScaler float maxDeltaAngle = dt * 10000.0f float alpha = constrain_float dt/ dt + 1.0f/ 2.0f* float M_PI*freq_cut 0.0f 1.0f "
ArduPilot 94 AC_PosControl.cpp Non-Control 2a5a133bbf AC_AttControl: remove deprecated trigger_xy method ""
ArduPilot 95 AC_PosControl.cpp Non-Control a580cd83e8 AC_PosControl: Fill _vel_desired.z for reporting " _vel_desired.z = constrain_float alt_change * dt _speed_down_cms _speed_up_cms _vel_desired.z = climb_rate_cms "
ArduPilot 96 AC_PosControl.cpp Non-Control 9ebd0e9960 AC_PosControl: reincarnate dead block of code ""
ArduPilot 97 AC_PosControl.cpp Non-Control aeecc46f7b AC_PosControl: remove unnecessary set of desired_accelThe desired_accel is set again 11 lines lower so this line did nothing. ""
ArduPilot 98 AC_PosControl.cpp Control 02f3f96310 AC_PosControl: Enable altitude limit checking. " if climb_rate_cms<0 && !_motors.limit.throttle_lower || force_descend || climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up "
ArduPilot 99 AC_PosControl.cpp Control 07a0388f25 AC_PosControl: move alt limit to set_alt_target_from_climb_rateThe alt limit is instead enforced when the target is set using theset_alt_target_from_climb_rate functionAlso updated comments " if _alt_max > 0 && _pos_target.z > _alt_max _pos_target.z = _alt_max _limit.pos_up = true "
ArduPilot 100 AC_PosControl.cpp Non-Control 14d76d158a AC_AttitudeControl: Correct comment. ""
ArduPilot 101 AC_PosControl.cpp Control 3faca88423 AC_PosControl: allow control of xy rate constraint behavior " void AC_PosControl::update_xy_controller xy_mode mode float ekfNavVelGainScaler pos_to_rate_xy mode dt ekfNavVelGainScaler pos_to_rate_xy XY_MODE_SLOW_POS_AND_VEL dt ekfNavVelGainScaler void AC_PosControl::pos_to_rate_xy xy_mode mode float dt float ekfNavVelGainScaler if mode == XY_MODE_SLOW_POS_AND_VEL float vel_total = pythagorous2 _vel_target.x _vel_target.y if vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR _vel_target.x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.x/vel_total _vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/vel_total else if mode == XY_MODE_POS_AND_VEL _vel_target.x += _vel_desired.x _vel_target.y += _vel_desired.y float vel_total = pythagorous2 _vel_target.x _vel_target.y if vel_total > _speed_cms _vel_target.x = _speed_cms * _vel_target.x/vel_total _vel_target.y = _speed_cms * _vel_target.y/vel_total "
ArduPilot 102 AC_PosControl.cpp Non-Control 186337f18e AC_PosControl: rename xy_mode enum valuesAlso added a few comments and fixed formatting " pos_to_rate_xy XY_MODE_POS_LIMITED_AND_VEL_FF dt ekfNavVelGainScaler if mode == XY_MODE_POS_LIMITED_AND_VEL_FF if mode == XY_MODE_POS_AND_VEL_FF "
ArduPilot 103 AC_PosControl.cpp Control e9bbe062f3 AC_PosControl: modify accel_to_lean_angles to apply filters before yaw rotation " float accel_north = _accel_target.x float accel_east = _accel_target.y static float last_accel_north = 0.0f static float last_accel_east = 0.0f float max_delta_accel = dt * 1700.0f if accel_north - last_accel_north > max_delta_accel accel_north = last_accel_north + max_delta_accel else if accel_north - last_accel_north < -max_delta_accel accel_north = last_accel_north - max_delta_accel last_accel_north = accel_north if accel_east - last_accel_east > max_delta_accel accel_east = last_accel_east + max_delta_accel else if accel_east - last_accel_east < -max_delta_accel accel_east = last_accel_east - max_delta_accel last_accel_east = accel_east static float accel_north_filtered = 0.0f static float accel_east_filtered = 0.0f accel_north_filtered += alpha * accel_north - accel_north_filtered accel_east_filtered += alpha * accel_east - accel_east_filtered accel_forward = accel_north_filtered*_ahrs.cos_yaw + accel_east_filtered*_ahrs.sin_yaw accel_right = -accel_north_filtered*_ahrs.sin_yaw + accel_east_filtered*_ahrs.cos_yaw _pitch_target = constrain_float fast_atan -accel_forward/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max float cos_pitch_target = cosf _pitch_target* float M_PI/18000 _roll_target = constrain_float fast_atan accel_right*cos_pitch_target/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max "
ArduPilot 104 AC_PosControl.cpp Control 8d4f0ec80c AC_PosControl: integrate PID input filter " _pid_alt_accel.set_dt _dt void AC_PosControl::set_dt_xy float dt_xy _dt_xy = dt_xy _pid_rate_lat.set_dt dt_xy _pid_rate_lon.set_dt dt_xy _pid_alt_accel.set_input_filter_d _accel_error.z p = _pid_alt_accel.get_p i = _pid_alt_accel.get_i d = _pid_alt_accel.get_d _pid_rate_lat.set_input_filter_d _vel_error.x _pid_rate_lon.set_input_filter_d _vel_error.y lat_i = _pid_rate_lat.get_i lon_i = _pid_rate_lon.get_i _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p + lat_i + _pid_rate_lat.get_d * ekfNavVelGainScaler _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p + lon_i + _pid_rate_lon.get_d * ekfNavVelGainScaler "
ArduPilot 105 AC_PosControl.cpp Control 349f1aeceb AC_PosControl: use 2-axis PI controller " AC_P& p_pos_xy AC_PI_2D& pi_vel_xy : _pi_vel_xy pi_vel_xy _pi_vel_xy.set_dt dt_xy _pi_vel_xy.set_integrator _accel_target _pi_vel_xy.set_integrator _accel_target Vector2f vel_xy_p vel_xy_i _pi_vel_xy.set_input _vel_error vel_xy_p = _pi_vel_xy.get_p if !_limit.accel_xy && !_motors.limit.throttle_upper vel_xy_i = _pi_vel_xy.get_i else vel_xy_i = _pi_vel_xy.get_i_shrink _accel_target.x = _accel_feedforward.x + vel_xy_p.x + vel_xy_i.x * ekfNavVelGainScaler _accel_target.y = _accel_feedforward.y + vel_xy_p.y + vel_xy_i.y * ekfNavVelGainScaler "
ArduPilot 106 AC_PosControl.cpp Non-Control 9866eaded1 AC_PosControl: rename p_alt_pos to p_pos_zAlso pid_alt_accel to pid_accel_z " AC_P& p_pos_z AC_P& p_vel_z AC_PID& pid_accel_z _p_pos_z p_pos_z _p_vel_z p_vel_z _pid_accel_z pid_accel_z _pid_accel_z.set_dt _dt linear_velocity = _accel_z_cms/_p_pos_z.kP stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP linear_distance = _accel_z_cms/ 2.0f*_p_pos_z.kP *_p_pos_z.kP _pid_accel_z.set_integrator _motors.get_throttle_out -_throttle_hover _leash_up_z = calc_leash_length _speed_up_cms _accel_z_cms _p_pos_z.kP _leash_down_z = calc_leash_length -_speed_down_cms _accel_z_cms _p_pos_z.kP _vel_target.z = AC_AttitudeControl::sqrt_controller _pos_error.z _p_pos_z.kP _accel_z_cms p = _p_vel_z.kP * _vel_error.z _pid_accel_z.set_input_filter_d _accel_error.z p = _pid_accel_z.get_p i = _pid_accel_z.get_integrator i = _pid_accel_z.get_i d = _pid_accel_z.get_d "
ArduPilot 107 AC_PosControl.cpp Non-Control 7de5bccc93 AC_PosControl: remove THR_HOVER parameterParameter is set by main code so no need to store to eeprom ""
ArduPilot 108 AC_PosControl.cpp Control 9871b95586 AC_PosControl: fix dt sanity checking " if dt > POSCONTROL_ACTIVE_TIMEOUT_SEC if dt >= POSCONTROL_ACTIVE_TIMEOUT_SEC "
ArduPilot 109 AC_PosControl.cpp Control 4408c1b935 AC_PosControl: 2d jerk constraint in accel_to_lean_angles " _distance_to_target 0.0f _accel_target_jerk_limited 0.0f 0.0f _accel_target_filtered 0.0f 0.0f Vector2f accel_in _accel_target.x _accel_target.y Vector2f accel_change = accel_in-_accel_target_jerk_limited float accel_change_length = accel_change.length if accel_change_length > max_delta_accel accel_change *= max_delta_accel/accel_change_length _accel_target_jerk_limited += accel_change _accel_target_filtered.x += alpha * _accel_target_jerk_limited.x - _accel_target_filtered.x _accel_target_filtered.y += alpha * _accel_target_jerk_limited.y - _accel_target_filtered.y accel_forward = _accel_target_filtered.x*_ahrs.cos_yaw + _accel_target_filtered.y*_ahrs.sin_yaw accel_right = -_accel_target_filtered.x*_ahrs.sin_yaw + _accel_target_filtered.y*_ahrs.cos_yaw "
ArduPilot 110 AC_PosControl.cpp Non-Control 965db2c7f7 AC_PosControl: add comments and defines for jerk limits " float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler "
ArduPilot 111 AC_PosControl.cpp Control 50d2e98aa4 AC_AttControl: init throttle_hover in constructor " _throttle_hover POSCONTROL_THROTTLE_HOVER "
ArduPilot 112 AC_PosControl.cpp Control 88ec13b10d AC_PosControl: fix build " if dt > POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f if dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f "
ArduPilot 113 AC_PosControl.cpp Control bb74b8dec8 AC_PosControl: fix twitch when entering RTLAlso removed slow_cpu flagFixed bug in update to _accel_target_jerk_limited. " _flags.reset_accel_to_lean_xy = true _flags.reset_accel_to_lean_xy = true _flags.reset_accel_to_lean_xy = true if _flags.reset_accel_to_lean_xy _accel_target_jerk_limited.x = _accel_target.x _accel_target_jerk_limited.y = _accel_target.y _accel_target_filtered.x = _accel_target.x _accel_target_filtered.y = _accel_target.y _flags.reset_accel_to_lean_xy = false _accel_target_jerk_limited += accel_change "
ArduPilot 114 AC_PosControl.cpp Control 9a3f48cc1f AC_PosControl: ensure throttle output above zero " int16_t thr_out = max int16_t p+i+d+_throttle_hover 1 _attitude_control.set_throttle_out thr_out true "
ArduPilot 115 AC_PosControl.cpp Non-Control 48fb487a8c AC_AttitudeControl: Formatting fix " _accel_target_filtered.y += alpha * _accel_target_jerk_limited.y - _accel_target_filtered.y "
ArduPilot 116 AC_PosControl.cpp Control d14893fcd5 AC_PosControl: don t limit throttle any more " int16_t thr_out = int16_t p+i+d+_throttle_hover "
ArduPilot 117 AC_PosControl.cpp Control 7abd02baf2 AC_PosControl: change int32/int16 to float in accel_to_throttle " float p i d _accel_error.z = _accel_error_filter.apply accel_target_z - z_accel_meas float thr_out = p+i+d+_throttle_hover "
ArduPilot 118 AC_PosControl.cpp Control 3e0dab7b2d AC_PosControl: remove accel error filter and set throttle output filter instead " _accel_error.z = accel_target_z - z_accel_meas _attitude_control.set_throttle_out thr_out true POSCONTROL_THROTTLE_CUTOFF_FREQ "
ArduPilot 119 AC_PosControl.cpp Control 7cb3c4ba39 AC_PosControl: add feed forward to Alt Hold " _vel_desired.z = 0.0f float jurk_z = _accel_z_cms * 2.0f float accel_z_max = min _accel_z_cms safe_sqrt 2.0f*fabs _vel_desired.z - climb_rate_cms *jurk_z _accel_last_z_cms += jurk_z * dt _accel_last_z_cms = min accel_z_max _accel_last_z_cms float vel_change_limit = _accel_last_z_cms * dt _vel_desired.z = constrain_float climb_rate_cms _vel_desired.z-vel_change_limit _vel_desired.z+vel_change_limit if _vel_desired.z<0 && !_motors.limit.throttle_lower || force_descend || _vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up _pos_target.z += _vel_desired.z * dt _vel_desired.z = constrain_float 0.0f _vel_desired.z-vel_change_limit _vel_desired.z+vel_change_limit _vel_target.z += _vel_desired.z _vel_desired.z = 0.0f "
ArduPilot 120 AC_PosControl.cpp Control 0aca3c4be6 AC_PosControl: add relax Alt Hold controllers " void AC_PosControl::relax_alt_hold_controllers float throttle_setting _pos_target.z = _inav.get_altitude _vel_desired.z = 0.0f _vel_target.z= _inav.get_velocity_z _vel_last.z = _inav.get_velocity_z _accel_feedforward.z = 0.0f _accel_last_z_cms = 0.0f _accel_target.z = - _ahrs.get_accel_ef_blended .z + GRAVITY_MSS * 100.0f _flags.reset_accel_to_throttle = true _pid_accel_z.set_integrator throttle_setting "
ArduPilot 121 AC_PosControl.cpp Non-Control a632a57d79 AC_PosControl: accessors to log z-axis vel and accel " _accel_target.z = _accel_feedforward.z + p accel_to_throttle _accel_target.z "
ArduPilot 122 AC_PosControl.cpp Control c2a6a0a9e2 AC_PosControl: add accel filter parameter " AP_GROUPINFO _ACC_XY_FILT 1 AC_PosControl _accel_xy_filt_hz POSCONTROL_ACCEL_FILTER_HZ float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO float accel_z_max = min _accel_z_cms safe_sqrt 2.0f*fabs _vel_desired.z - climb_rate_cms *jerk_z _accel_last_z_cms += jerk_z * dt float freq_cut = min _accel_xy_filt_hz 5.0f*ekfNavVelGainScaler "
ArduPilot 123 AC_PosControl.cpp Control 87500d9d70 AC_AttitudeControl: use new lowpass filter " _vel_error_filter.set_cutoff_frequency POSCONTROL_VEL_ERROR_CUTOFF_FREQ _vel_error.z = _vel_error_filter.apply _vel_target.z - curr_vel.z _dt "
ArduPilot 124 AC_PosControl.cpp Control 738b1967ad AC_PosControl: use LowPassFilterVector2f " _accel_target_filter POSCONTROL_ACCEL_FILTER_HZ _accel_target_filter.reset Vector2f _accel_target.x _accel_target.y _accel_target_filter.set_cutoff_frequency min _accel_xy_filt_hz 5.0f*ekfNavVelGainScaler Vector2f accel_target_filtered = _accel_target_filter.apply _accel_target_jerk_limited dt accel_forward = accel_target_filtered.x*_ahrs.cos_yaw + accel_target_filtered.y*_ahrs.sin_yaw accel_right = -accel_target_filtered.x*_ahrs.sin_yaw + accel_target_filtered.y*_ahrs.cos_yaw "
ArduPilot 125 AC_PosControl.cpp Control 12957867fd AC_PosControl: fix thr twitch when changing modes " curr_vel_z -= _vel_desired.z "
ArduPilot 126 AC_PosControl.cpp Control 31edd6a72b AC_PosControl: add_takeoff_climb_rate methodThis function simply increments the current altitude target given aclimb rate and dt " void AC_PosControl::add_takeoff_climb_rate float climb_rate_cms float dt _pos_target.z += climb_rate_cms * dt "
ArduPilot 127 AC_PosControl.cpp Control 1bca81eaed AC_AttitudeControl: compiler warnings: apply is_zero(float) or is_equal(float) and float to doubles " #include speed_down = -fabsf speed_down if fabsf _speed_down_cms-speed_down > 1.0f || fabsf _speed_up_cms-speed_up > 1.0f if fabsf _accel_z_cms-accel_cmss > 1.0f float accel_z_max = min _accel_z_cms safe_sqrt 2.0f*fabsf _vel_desired.z - climb_rate_cms *jerk_z if fabsf curr_vel_z < linear_velocity if fabsf _accel_cms-accel_cmss > 1.0f if fabsf _speed_cms-speed_cms > 1.0f if kP <= 0.0f || _accel_cms <= 0.0f || AP_Math::is_zero vel_total _pitch_target = constrain_float fast_atan -accel_forward/ GRAVITY_MSS * 100 * 18000/ float M_PI -lean_angle_max lean_angle_max _roll_target = constrain_float fast_atan accel_right*cos_pitch_target/ GRAVITY_MSS * 100 * 18000/ float M_PI -lean_angle_max lean_angle_max "
ArduPilot 128 AC_PosControl.cpp Control 326b0b33ea AC_AttitudeControl: revert AP_Math class change " if kP <= 0.0f || _accel_cms <= 0.0f || is_zero vel_total "
ArduPilot 129 AC_PosControl.cpp Control 84ac721340 AC_AttitudeControl: use M_PI_F instead of (float)M_PI " _pitch_target = constrain_float fast_atan -accel_forward/ GRAVITY_MSS * 100 * 18000/M_PI_F -lean_angle_max lean_angle_max float cos_pitch_target = cosf _pitch_target*M_PI_F/18000 _roll_target = constrain_float fast_atan accel_right*cos_pitch_target/ GRAVITY_MSS * 100 * 18000/M_PI_F -lean_angle_max lean_angle_max "
ArduPilot 130 AC_PosControl.cpp Control 872583f441 AC_AttitudeControl: removed fast_atan " _pitch_target = constrain_float atanf -accel_forward/ GRAVITY_MSS * 100 * 18000/M_PI_F -lean_angle_max lean_angle_max _roll_target = constrain_float atanf accel_right*cos_pitch_target/ GRAVITY_MSS * 100 * 18000/M_PI_F -lean_angle_max lean_angle_max "
ArduPilot 131 AC_PosControl.cpp Control d1808c645d AC_PosControl: recalc leash length on speed or accel changeThis resolves an issue in which do-set-speed would not take effect untilone waypoint too late in a mission. " calc_leash_length_z calc_leash_length_z calc_leash_length_xy calc_leash_length_xy "
ArduPilot 132 AC_PosControl.cpp Non-Control e87ca6de6f AC_PosControl: Change motors.get_throttle_outto get_throttle to follow function renaming in AP_Motors " _pid_accel_z.set_integrator _motors.get_throttle -_throttle_hover "
ArduPilot 133 AC_PosControl.cpp Control 521dae1c65 AC_AttitudeControl: use set_desired_rate() on PID controllersthis sets them up for logging of PIDs " _pid_accel_z.set_desired_rate accel_target_z "
ArduPilot 134 AC_PosControl.cpp Control 11fee21f06 AC_PosControl: init accel_last_z_cms " _accel_last_z_cms 0.0f "
ArduPilot 135 AC_PosControl.cpp Control 382f5d087f AC_PosControl: explicitly set set_alt_target_from_climb_rate params " set_alt_target_from_climb_rate _vel_desired.z dt false "
ArduPilot 136 AC_PosControl.cpp Control d2f93dd379 AC_PosControl: init flagsResolves warning from Coverity " _flags.recalc_leash_xy = true _flags.freeze_ff_xy = true _flags.freeze_ff_z = true _limit.pos_up = true _limit.pos_down = true _limit.vel_up = true _limit.vel_down = true _limit.accel_xy = true "
ArduPilot 137 AC_PosControl.cpp Non-Control 14882bc6a8 AC_AttitudeControl: Remove unused takeoff jump #define " _pos_target.z = curr_pos.z "
ArduPilot 138 AC_PosControl.cpp Non-Control d49f10d2e5 AC_AttitudeControl: standardize inclusion of libaries headersThis commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to so the notation #include is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source then we use the notation #include <> with the path relative to libraries folder.Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time. " #include #include AC_PosControl.h #include "
ArduPilot 139 AC_PosControl.cpp Control b58cc7ea8d AC_PosControl: move accel constraint to accel_to_lean_angles " void AC_PosControl::accel_to_lean_angles float dt float ekfNavVelGainScaler float accel_total float accel_right accel_forward float lean_angle_max = _attitude_control.lean_angle_max "
ArduPilot 140 AC_PosControl.cpp Control cf5db31053 AC_PosControl: allow limiting lean angle to avoid alt loss " void AC_PosControl::update_xy_controller xy_mode mode float ekfNavVelGainScaler bool use_althold_lean_angle accel_to_lean_angles dt ekfNavVelGainScaler use_althold_lean_angle accel_to_lean_angles dt ekfNavVelGainScaler false void AC_PosControl::accel_to_lean_angles float dt float ekfNavVelGainScaler bool use_althold_lean_angle float accel_max = POSCONTROL_ACCEL_XY_MAX if use_althold_lean_angle accel_max = min accel_max GRAVITY_MSS * 100.0f * sinf ToRad constrain_float _attitude_control.get_althold_lean_angle_max 1000 8000 /100.0f if accel_total > accel_max && accel_total > 0.0f _accel_target.x = accel_max * _accel_target.x/accel_total _accel_target.y = accel_max * _accel_target.y/accel_total "
ArduPilot 141 AC_PosControl.cpp Control f55c31a157 AC_PosControl: add alt hold without feed forward " if climb_rate_cms<0 && !_motors.limit.throttle_lower || force_descend || climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up _pos_target.z += climb_rate_cms * dt if _alt_max > 0 && _pos_target.z > _alt_max _pos_target.z = _alt_max _limit.pos_up = true _vel_desired.z = 0.0f void AC_PosControl::set_alt_target_from_climb_rate_ff float climb_rate_cms float dt bool force_descend "
ArduPilot 142 AC_PosControl.cpp Control 245f7ce268 AC_PosControl: allow desired vel z to be above speed limit " _limit.vel_up = false _limit.vel_down = false if _vel_target.z < _speed_down_cms _vel_target.z = _speed_down_cms _limit.vel_down = true if _vel_target.z > _speed_up_cms _vel_target.z = _speed_up_cms _limit.vel_up = true "
ArduPilot 143 AC_PosControl.cpp Control aec66c5db6 AC_PosControl: faster z-axis slowdown when over speed " float accel_z_cms = _accel_z_cms if _vel_desired.z < _speed_down_cms && !is_zero _speed_down_cms accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms if _vel_desired.z > _speed_up_cms && !is_zero _speed_up_cms accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms accel_z_cms = constrain_float accel_z_cms 0.0f 750.0f float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO float accel_z_max = min accel_z_cms safe_sqrt 2.0f*fabsf _vel_desired.z - climb_rate_cms *jerk_z "
ArduPilot 144 AC_PosControl.cpp Control 895a40893d AC_PosControl: use_desvel_ff flag addedThis allows turning on/off desired velocity feedforward without setting desired_vel.z to zero. Setting desired_vel.z to zero has the side effect of disrupting the landing detection which needs to know if we are trying to descend " _flags.use_desvel_ff_z = true _flags.use_desvel_ff_z = false _flags.use_desvel_ff_z = false _vel_desired.z = climb_rate_cms _flags.use_desvel_ff_z = true _flags.use_desvel_ff_z = false if _flags.use_desvel_ff_z curr_vel_z -= _vel_desired.z if _flags.use_desvel_ff_z _vel_target.z += _vel_desired.z _flags.use_desvel_ff_z = false "
ArduPilot 145 AC_PosControl.cpp Non-Control ee0abb1750 AC_PosControl: add set_jerk_xy " _jerk_cmsss POSCONTROL_JERK_LIMIT_CMSSS float max_delta_accel = dt * _jerk_cmsss "
ArduPilot 146 AC_PosControl.cpp Control 550ba478c9 AC_PosControl: add shift_pos_xy_target " void AC_PosControl::shift_pos_xy_target float x_cm float y_cm _pos_target.x += x_cm _pos_target.y += y_cm if !is_zero x_cm || !is_zero y_cm freeze_ff_xy "
ArduPilot 147 AC_PosControl.cpp Non-Control 831d8acca5 Remove use of PROGMEMNow variables don t have to be declared with PROGMEM anymore so removethem. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i s/ PROGMEM / /g git grep -l -z PROGMEM | xargs -0 sed -i s/PROGMEM//g The 2 commands were done so we don t leave behind spurious spaces.AVR-specific places were not changed. " const AP_Param::GroupInfo AC_PosControl::var_info = "
ArduPilot 148 AC_PosControl.cpp Control 323a527734 AC_PosControl: velocity controller uses feed-forward althold " set_alt_target curr_pos.z set_desired_velocity curr_vel set_alt_target_from_climb_rate_ff _vel_desired.z dt false "
ArduPilot 149 AC_PosControl.cpp Control dbc8ce1d69 AC_PosControl: default z-axis controller to 400hzNo functional change as vehicle code always sets it explicitely " _dt POSCONTROL_DT_400HZ "
ArduPilot 150 AC_PosControl.cpp Control c9340dbeb6 AC_PosControl: run velocity controller z-axis at 400hz " float dt = now - _last_update_xy_ms *0.001f if dt >= get_dt_xy if dt >= 0.2f dt = 0.0f calc_leash_length_xy desired_vel_to_pos dt pos_to_rate_xy XY_MODE_POS_LIMITED_AND_VEL_FF dt ekfNavVelGainScaler rate_to_accel_xy dt ekfNavVelGainScaler accel_to_lean_angles dt ekfNavVelGainScaler false _last_update_xy_ms = now set_alt_target_from_climb_rate_ff _vel_desired.z _dt false "
ArduPilot 151 AC_PosControl.cpp Control ea08b6115d AC_AttControl: use millis/micros/panic functions " return AP_HAL::millis - _last_update_z_ms <= POSCONTROL_ACTIVE_TIMEOUT_MS uint32_t now = AP_HAL::millis return AP_HAL::millis - _last_update_xy_ms <= POSCONTROL_ACTIVE_TIMEOUT_MS uint32_t now = AP_HAL::millis uint32_t now = AP_HAL::millis uint32_t now = AP_HAL::millis "
ArduPilot 152 AC_PosControl.cpp Non-Control 2591261af6 Global: rename min and max macros to uppercaseThe problem with using min() and max() is that they conflict with someC++ headers. Name the macros in uppercase instead. We may go case bycase later converting them to be typesafe.Changes generated with:git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)max(/\1MAX(/g git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)min(/\1MIN(/g " float accel_z_max = MIN accel_z_cms safe_sqrt 2.0f*fabsf _vel_desired.z - climb_rate_cms *jerk_z _accel_last_z_cms = MIN accel_z_max _accel_last_z_cms accel_max = MIN accel_max GRAVITY_MSS * 100.0f * sinf ToRad constrain_float _attitude_control.get_althold_lean_angle_max 1000 8000 /100.0f _accel_target_filter.set_cutoff_frequency MIN _accel_xy_filt_hz 5.0f*ekfNavVelGainScaler accel_x_cmss = GRAVITY_MSS * 100 * - _ahrs.cos_yaw * _ahrs.sin_pitch / MAX _ahrs.cos_pitch 0.5f - _ahrs.sin_yaw * _ahrs.sin_roll / MAX _ahrs.cos_roll 0.5f accel_y_cmss = GRAVITY_MSS * 100 * - _ahrs.sin_yaw * _ahrs.sin_pitch / MAX _ahrs.cos_pitch 0.5f + _ahrs.cos_yaw * _ahrs.sin_roll / MAX _ahrs.cos_roll 0.5f "
ArduPilot 153 AC_PosControl.cpp Non-Control 5148e41c1a AP_Math: Cleaned macro definitionsMoved Definitions into a separate header. Replaced PI with M_PI andremoved the M_PI_*_F macros. " _pitch_target = constrain_float atanf -accel_forward/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max float cos_pitch_target = cosf _pitch_target*M_PI/18000 _roll_target = constrain_float atanf accel_right*cos_pitch_target/ GRAVITY_MSS * 100 * 18000/M_PI -lean_angle_max lean_angle_max "
ArduPilot 154 AC_PosControl.cpp Control 15be80a25d AC_PosControl: accel_to_throttle outputs 0 to 1 " float thr_out = p+i+d /1000.0f +_throttle_hover "
ArduPilot 155 AC_PosControl.cpp Control c64a505906 AC_PosControl: relax_alt_hold_controllers accepts throttle in 0 to 1 range " _pid_accel_z.set_integrator throttle_setting*1000.0f "
ArduPilot 156 AC_PosControl.cpp Control 23a64e1227 AC_AttitudeControl: fixed accel limit trigonometryLeonard had mentioned the limit should be tan(angle) not sin(angle). Inoticed this one was wrong. " accel_max = MIN accel_max GRAVITY_MSS * 100.0f * tanf ToRad constrain_float _attitude_control.get_althold_lean_angle_max 1000 8000 /100.0f "
ArduPilot 157 AC_PosControl.cpp Control 41661f815f AP_Math: Replace the pythagorous* functions with a variadic templateThe new function can deal with a variable number of function parameters.Additionally I renamed the functions to norm() because this is thestandard name used in several other projects. " float vel_total = norm curr_vel.x curr_vel.y _distance_to_target = norm _pos_error.x _pos_error.y float vel_total = norm _vel_target.x _vel_target.y float vel_total = norm _vel_target.x _vel_target.y accel_total = norm _accel_target.x _accel_target.y "
ArduPilot 158 AC_PosControl.cpp Control 1639e22b74 AC_AttitudeControl: set desired_vel for reporting purposes even when not used " if !is_zero dt float climb_rate_cms = constrain_float alt_change/dt _speed_down_cms _speed_up_cms _pos_target.z += climb_rate_cms*dt _vel_desired.z = climb_rate_cms "
ArduPilot 159 AC_PosControl.cpp Non-Control dcbb071c07 AC_PosControl: Move hover throttle calculation to AP_Motors " _pid_accel_z.set_integrator _motors.get_throttle -_motors.get_throttle_hover float thr_out = p+i+d /1000.0f +_motors.get_throttle_hover "
ArduPilot 160 AC_PosControl.cpp Control d1b28aaed9 AC_AttitudeControl: fixed factor of 1000 error in init_takeoff " _pid_accel_z.set_integrator _motors.get_throttle -_motors.get_throttle_hover *1000.0f "
ArduPilot 161 AC_PosControl.cpp Non-Control 630e5378da AC_PosControl: add get_horizontal_error " float AC_PosControl::get_horizontal_error const return norm _pos_error.x _pos_error.y "
ArduPilot 162 AC_PosControl.cpp Control 34055944cd AC_PosControl: fix relax_alt_hold_controllers " _pid_accel_z.set_integrator throttle_setting-_motors.get_throttle_hover *1000.0f "
ArduPilot 163 AC_PosControl.cpp Control fdcdcb0033 AC_PosControl: check Z-axis accel imax can always overpower hover throttleThis removes the possibility of the vehicle constantly climbing if the hover throttle becomes a very large value " if _motors.get_throttle_hover * 1000.0f > _pid_accel_z.imax _pid_accel_z.imax _motors.get_throttle_hover * 1000.0f "
ArduPilot 164 AC_PosControl.cpp Control 77dbf99cee AC_PosControl: fix bug related to ekfNavVelGainScaler " _vel_target.x = kP * _pos_error.x _vel_target.y = kP * _pos_error.y "
ArduPilot 165 AC_PosControl.cpp Control 5a8db4f271 AC_PosControl: add interface to override horizontal velocity process variable " _vel_target.z = _inav.get_velocity_z const Vector3f curr_pos = _inav.get_position Vector3f curr_vel = _inav.get_velocity if _flags.vehicle_horiz_vel_override _flags.vehicle_horiz_vel_override = false else _vehicle_horiz_vel.x = _inav.get_velocity .x _vehicle_horiz_vel.y = _inav.get_velocity .y _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y "
ArduPilot 166 AC_PosControl.cpp Non-Control 152edf7189 Global: remove mode line from headersUsing a global .dir-locals.el file is a better alternative thanreincluding the same emacs header in every file of the project. ""
ArduPilot 167 AC_PosControl.cpp Control 42672de606 AC_PosControl: set Alt_Hold filter for PID not just DWe normally don t use the D term here so setting the filter for PID notjust D lets us smooth the throttle response. " _pid_accel_z.set_input_filter_all _accel_error.z "
ArduPilot 168 AC_PosControl.cpp Control 1012333eef AC_PosControl: add ekf position reset handlingPreviously this was in AC_WPNav and used only for loiter but it should work for any flight modes that use horizontal or vertical position control " void AC_PosControl::shift_alt_target float z_cm _pos_target.z += z_cm if !is_zero z_cm freeze_ff_z init_ekf_z_reset check_for_ekf_z_reset init_ekf_xy_reset check_for_ekf_xy_reset init_ekf_xy_reset init_ekf_z_reset check_for_ekf_xy_reset void AC_PosControl::init_ekf_xy_reset Vector2f pos_shift _ekf_xy_reset_ms = _ahrs.getLastPosNorthEastReset pos_shift void AC_PosControl::check_for_ekf_xy_reset Vector2f pos_shift uint32_t reset_ms = _ahrs.getLastPosNorthEastReset pos_shift if reset_ms != _ekf_xy_reset_ms shift_pos_xy_target pos_shift.x * 100.0f pos_shift.y * 100.0f _ekf_xy_reset_ms = reset_ms void AC_PosControl::init_ekf_z_reset float alt_shift _ekf_z_reset_ms = _ahrs.getLastPosDownReset alt_shift void AC_PosControl::check_for_ekf_z_reset float alt_shift uint32_t reset_ms = _ahrs.getLastPosDownReset alt_shift if reset_ms != _ekf_z_reset_ms shift_alt_target -alt_shift * 100.0f _ekf_z_reset_ms = reset_ms "
ArduPilot 169 AC_PosControl.cpp Non-Control 421524951f AC_PosControl: remove unnecessary parentheses " if !_limit.accel_xy && !_motors.limit.throttle_upper "
ArduPilot 170 AC_PosControl.cpp Non-Control ff042528fe AC_PosControl: remove alt_maxAC_Avoidance enforces the altitude limit ""
ArduPilot 171 AC_PosControl.cpp Non-Control 1345bf8737 AC_AttitudeControl: added support for AP_AHRS_Viewthis allows for tailsitters with a different attitude view " AC_PosControl::AC_PosControl const AP_AHRS_View& ahrs const AP_InertialNav& inav "
ArduPilot 172 AC_PosControl.cpp Control 132979b12f AC_PosControl: protect against POS_Z_P ACCEL_Z_P divide-by-zero " if _p_pos_z.kP <= 0.0f || _accel_z_cms <= 0.0f stopping_point.z = curr_pos_z return "
ArduPilot 173 AC_PosControl.cpp Control 3a397584a1 AC_PosControl: z-axis stopping point up to 3m above vehicleStopping point while descending remains at 2m for safety " stopping_point.z = constrain_float stopping_point.z curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX "
ArduPilot 175 AC_AttitudeControl.cpp Control 89b7e6b1c8 AC_AttControl: implement angle_boost " if apply_angle_boost _angle_boost = 0 int16_t AC_AttitudeControl::get_angle_boost int16_t throttle_pwm throttle_out = constrain_float float throttle_pwm-_motors.throttle_min * temp + _motors.throttle_min _motors.throttle_min 1000 _angle_boost = throttle_out - throttle_pwm "
ArduPilot 176 AC_AttitudeControl.cpp Control 0e0a15f4a8 AC_AttControl: add high level angle controllers " void AC_AttitudeControl::init_targets _angle_ef_target.x = _ahrs.roll_sensor _angle_ef_target.y = _ahrs.pitch_sensor _angle_ef_target.z = _ahrs.yaw_sensor void AC_AttitudeControl::angleef_rp_rateef_y float roll_angle_ef float pitch_angle_ef float yaw_rate_ef _angle_ef_target.x = roll_angle_ef _angle_ef_target.y = pitch_angle_ef angle_to_rate_ef_roll angle_to_rate_ef_pitch _rate_stab_ef_target.z = yaw_rate_ef rate_stab_ef_to_rate_ef_yaw rate_ef_targets_to_bf void AC_AttitudeControl::angleef_rpy float roll_angle_ef float pitch_angle_ef float yaw_rate_ef _angle_ef_target.x = roll_angle_ef _angle_ef_target.y = pitch_angle_ef angle_to_rate_ef_roll angle_to_rate_ef_pitch angle_to_rate_ef_yaw rate_ef_targets_to_bf "
ArduPilot 177 AC_AttitudeControl.cpp Non-Control 9fe4d883d0 AC_AttControl: accessor for lean angle max " _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float _angle_ef_target.y -_aparm.angle_max _aparm.angle_max "
ArduPilot 178 AC_AttitudeControl.cpp Non-Control ba3ba9e157 AC_AttControl: bug fix for yaw target handling " void AC_AttitudeControl::angleef_rpy float roll_angle_ef float pitch_angle_ef float yaw_angle_ef _angle_ef_target.z = yaw_angle_ef "
ArduPilot 179 AC_AttitudeControl.cpp Control 4003b4da9b AC_AttControl: leonard s body frame rate controller " rate_ef_targets_to_bf _rate_ef_target _rate_bf_target rate_ef_targets_to_bf _rate_ef_target _rate_bf_target void AC_AttitudeControl::ratebf_rpy float roll_rate_bf float pitch_rate_bf float yaw_rate_bf rate_stab_bf_update_error _rate_stab_bf_target.x = roll_rate_bf _rate_stab_bf_target.y = pitch_rate_bf _rate_stab_bf_target.z = yaw_rate_bf rate_stab_bf_to_rate_bf_roll rate_stab_bf_to_rate_bf_pitch rate_stab_bf_to_rate_bf_yaw void AC_AttitudeControl::rate_stab_bf_update_error _rate_stab_bf_error.x += _rate_stab_bf_target.x - _ins.get_gyro .x * AC_ATTITUDE_CONTROL_DEGX100 * _dt _rate_stab_bf_error.x = constrain_float _rate_stab_bf_error.x -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX _rate_stab_bf_error.y += _rate_stab_bf_target.y - _ins.get_gyro .y * AC_ATTITUDE_CONTROL_DEGX100 * _dt _rate_stab_bf_error.y = constrain_float _rate_stab_bf_error.y -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX _rate_stab_bf_error.z += _rate_stab_bf_target.z - _ins.get_gyro .z * AC_ATTITUDE_CONTROL_DEGX100 * _dt _rate_stab_bf_error.z = constrain_float _rate_stab_bf_error.z -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX void AC_AttitudeControl::rate_ef_targets_to_bf Vector3f rate_ef_target Vector3f &rate_bf_target rate_bf_target.x = rate_ef_target.x - _sin_pitch * rate_ef_target.z rate_bf_target.y = _cos_roll * rate_ef_target.y + _sin_roll * _cos_pitch * rate_ef_target.z rate_bf_target.z = _cos_pitch * _cos_roll * rate_ef_target.z - _sin_roll * rate_ef_target.y "
ArduPilot 180 AC_AttitudeControl.cpp Control 468be05867 AC_AttControl: make ahrs ins objects const " void AC_AttitudeControl::rate_ef_targets_to_bf const Vector3f& rate_ef_target Vector3f& rate_bf_target "
ArduPilot 181 AC_AttitudeControl.cpp Control a2b017abd2 AC_AttControl: add rateef_rpyrateef_rpy takes earth frame rates and applies them to the earth frameangle targets and then converts them to body frame rates " void AC_AttitudeControl::rateef_rpy float roll_rate_ef float pitch_rate_ef float yaw_rate_ef _rate_stab_ef_target.x = roll_rate_ef _rate_stab_ef_target.y = pitch_rate_ef _rate_stab_ef_target.z = yaw_rate_ef rate_stab_ef_to_rate_ef_roll rate_stab_ef_to_rate_ef_pitch rate_stab_ef_to_rate_ef_yaw rate_ef_targets_to_bf _rate_ef_target _rate_bf_target "
ArduPilot 182 AC_AttitudeControl.cpp Control 2dae0d68c5 AC_AttControl: add RATE_RP_MAX RATE_Y_MAX paramsThese replace the ANGLE_RATE_MAX parameter from the main code " AP_GROUPINFO RATE_RP_MAX 0 AC_AttitudeControl _angle_rate_rp_max AC_ATTITUDE_CONTROL_ANGLE_RATE_RP_MAX_DEFAULT AP_GROUPINFO RATE_Y_MAX 1 AC_AttitudeControl _angle_rate_y_max AC_ATTITUDE_CONTROL_ANGLE_RATE_Y_MAX_DEFAULT if _flags.limit_angle_to_rate_request _rate_ef_target.x = constrain_float _rate_ef_target.x -_angle_rate_rp_max _angle_rate_rp_max if _flags.limit_angle_to_rate_request _rate_ef_target.y = constrain_float _rate_ef_target.y -_angle_rate_rp_max _angle_rate_rp_max if _flags.limit_angle_to_rate_request _rate_ef_target.y = constrain_float _rate_ef_target.y -_angle_rate_y_max _angle_rate_y_max "
ArduPilot 183 AC_AttitudeControl.cpp Control 2db24659d0 AC_AttControl: add slew_yaw " AP_GROUPINFO RATE_RP_MAX 0 AC_AttitudeControl _angle_rate_rp_max AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT AP_GROUPINFO RATE_Y_MAX 1 AC_AttitudeControl _angle_rate_y_max AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT AP_GROUPINFO SLEW_YAW 2 AC_AttitudeControl _slew_yaw AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT void AC_AttitudeControl::angleef_rpy float roll_angle_ef float pitch_angle_ef float yaw_angle_ef bool slew_yaw if slew_yaw && _angle_ef_target.z != yaw_angle_ef float slew = _slew_yaw * _dt _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + constrain_float wrap_180_cd_float yaw_angle_ef - _angle_ef_target.z -slew slew "
ArduPilot 184 AC_AttitudeControl.cpp Control eda376c7f6 AC_AttControl: init_targets() clears body frame angle errorsThis should ensure that we don t get sudden jerks when entering acromode " _rate_stab_bf_error.x = 0 _rate_stab_bf_error.y = 0 _rate_stab_bf_error.z = 0 "
ArduPilot 185 AC_AttitudeControl.cpp Non-Control d76180d605 AC_AttControl: use trig values from ahrs " rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch * rate_ef_target.z rate_bf_target.y = _ahrs.cos_roll * rate_ef_target.y + _ahrs.sin_roll * _ahrs.cos_pitch * rate_ef_target.z rate_bf_target.z = _ahrs.cos_pitch * _ahrs.cos_roll * rate_ef_target.z - _ahrs.sin_roll * rate_ef_target.y float temp = _ahrs.cos_pitch * _ahrs.cos_roll "
ArduPilot 186 AC_AttitudeControl.cpp Non-Control 5f89e9e746 AC_AttControl: bug fix to yaw rate limit " _rate_ef_target.z = constrain_float _rate_ef_target.z -_angle_rate_y_max _angle_rate_y_max "
ArduPilot 187 AC_AttitudeControl.cpp Control 98224db1e4 AC_AttControl: Leaonard s rate feedforwardAlso saves 24bytes of RAM " _angle_bf_error.x = 0 _angle_bf_error.y = 0 _angle_bf_error.z = 0 _rate_bf_feedforward.x = 0 _rate_bf_feedforward.y = 0 _rate_bf_feedforward.z = 0 Vector3f rate_ef_feedforward Vector3f angle_ef_error angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.y = wrap_180_cd_float _angle_ef_target.y - _ahrs.pitch_sensor rate_ef_feedforward.z = yaw_rate_ef _angle_ef_target.z += yaw_rate_ef * _dt _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z angle_ef_error.z = wrap_180_cd_float _angle_ef_target.z - _ahrs.yaw_sensor angle_ef_error.z = constrain_float angle_ef_error.z -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX _angle_ef_target.z = wrap_360_cd_float angle_ef_error.z + _ahrs.yaw_sensor rate_ef_targets_to_bf angle_ef_error _angle_bf_error rate_ef_targets_to_bf rate_ef_feedforward _rate_bf_feedforward update_stab_rate_bf_targets _rate_bf_target += _rate_bf_feedforward void AC_AttitudeControl::angleef_rpy float roll_angle_ef float pitch_angle_ef float yaw_angle_ef bool slew_yaw Vector3f angle_ef_error _angle_ef_target.x = roll_angle_ef _angle_ef_target.y = pitch_angle_ef _angle_ef_target.z = yaw_angle_ef angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.y = wrap_180_cd_float _angle_ef_target.y - _ahrs.pitch_sensor angle_ef_error.z = wrap_180_cd_float _angle_ef_target.z - _ahrs.yaw_sensor rate_ef_targets_to_bf angle_ef_error _angle_bf_error update_stab_rate_bf_targets if slew_yaw _rate_bf_target.z = constrain_float _rate_bf_target.z -_slew_yaw _slew_yaw void AC_AttitudeControl::rateef_rpy float roll_rate_ef float pitch_rate_ef float yaw_rate_ef Vector3f rate_ef_feedforward Vector3f angle_ef_error rate_ef_feedforward.x = roll_rate_ef rate_ef_feedforward.y = pitch_rate_ef rate_ef_feedforward.z = yaw_rate_ef _angle_ef_target.x += roll_rate_ef * _dt angle_ef_error.x = wrap_180_cd _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.x = constrain_float angle_ef_error.x -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX _angle_ef_target.x = wrap_180_cd angle_ef_error.x + _ahrs.roll_sensor _angle_ef_target.y += pitch_rate_ef * _dt angle_ef_error.y = wrap_180_cd _angle_ef_target.y - _ahrs.pitch_sensor angle_ef_error.y = constrain_float angle_ef_error.y -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX _angle_ef_target.y = wrap_180_cd angle_ef_error.y + _ahrs.pitch_sensor _angle_ef_target.z += yaw_rate_ef * _dt _angle_ef_target.z = wrap_360_cd _angle_ef_target.z angle_ef_error.z = wrap_180_cd _angle_ef_target.z - _ahrs.yaw_sensor angle_ef_error.z = constrain_float angle_ef_error.z -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX _angle_ef_target.z = wrap_360_cd angle_ef_error.z + _ahrs.yaw_sensor rate_ef_targets_to_bf angle_ef_error _angle_bf_error rate_ef_targets_to_bf rate_ef_feedforward _rate_bf_feedforward update_stab_rate_bf_targets _rate_bf_target += _rate_bf_feedforward void AC_AttitudeControl::ratebf_rpy float roll_rate_bf float pitch_rate_bf float yaw_rate_bf update_stab_rate_bf_errors update_stab_rate_bf_targets _rate_bf_feedforward.x = roll_rate_bf _rate_bf_feedforward.y = pitch_rate_bf _rate_bf_feedforward.z = yaw_rate_bf _rate_bf_target += _rate_bf_feedforward void AC_AttitudeControl::rate_controller_run _motor_roll = rate_bf_to_motor_roll _rate_bf_target.x _motor_pitch = rate_bf_to_motor_pitch _rate_bf_target.y _motor_yaw = rate_bf_to_motor_yaw _rate_bf_target.z void AC_AttitudeControl::rate_ef_targets_to_bf const Vector3f& rate_ef_target Vector3f& rate_bf_target rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch * rate_ef_target.z rate_bf_target.y = _ahrs.cos_roll * rate_ef_target.y + _ahrs.sin_roll * _ahrs.cos_pitch * rate_ef_target.z rate_bf_target.z = -_ahrs.sin_roll * rate_ef_target.y + _ahrs.cos_pitch * _ahrs.cos_roll * rate_ef_target.z void AC_AttitudeControl::rate_bf_targets_to_ef const Vector3f& rate_bf_target Vector3f& rate_ef_target rate_ef_target.x = rate_bf_target.x - _ahrs.sin_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * rate_bf_target.y - _ahrs.cos_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * rate_bf_target.z rate_ef_target.y = _ahrs.cos_roll * rate_bf_target.y - _ahrs.sin_roll * rate_bf_target.z rate_ef_target.z = _ahrs.sin_roll / _ahrs.cos_pitch * rate_bf_target.y + _ahrs.cos_roll / _ahrs.cos_pitch * rate_bf_target.z void AC_AttitudeControl::update_stab_rate_bf_errors _angle_bf_error.x += _rate_bf_feedforward.x - _ins.get_gyro .x * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.x = constrain_float _angle_bf_error.x -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX _angle_bf_error.y += _rate_bf_feedforward.y - _ins.get_gyro .y * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.y = constrain_float _angle_bf_error.y -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX _angle_bf_error.z += _rate_bf_feedforward.z - _ins.get_gyro .z * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.z = constrain_float _angle_bf_error.z -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX void AC_AttitudeControl::update_stab_rate_bf_targets _rate_bf_target.x = _pi_angle_roll.kP * _angle_bf_error.x if _flags.limit_angle_to_rate_request _rate_bf_target.x = constrain_float _rate_bf_target.x -_angle_rate_rp_max _angle_rate_rp_max _rate_bf_target.y = _pi_angle_pitch.kP * _angle_bf_error.y if _flags.limit_angle_to_rate_request _rate_bf_target.y = constrain_float _rate_bf_target.y -_angle_rate_rp_max _angle_rate_rp_max _rate_bf_target.z = _pi_angle_yaw.kP * _angle_bf_error.z if _flags.limit_angle_to_rate_request _rate_bf_target.z = constrain_float _rate_bf_target.z -_angle_rate_y_max _angle_rate_y_max "
ArduPilot 188 AC_AttitudeControl.cpp Non-Control f216cffb77 AC_AttControl: use motor accessors to set roll pitch yaw thrSaves 8bytes of RAM " _motors.set_roll rate_bf_to_motor_roll _rate_bf_target.x _motors.set_pitch rate_bf_to_motor_pitch _rate_bf_target.y _motors.set_yaw rate_bf_to_motor_yaw _rate_bf_target.z _motors.set_throttle get_angle_boost throttle_out _motors.set_throttle throttle_out "
ArduPilot 189 AC_AttitudeControl.cpp Control 65c2fc0cc6 AC_AttControl: ACRO fixes " rateef_yaw_update_angle_and_error yaw_rate_ef angle_ef_error rateef_roll_update_angle_and_error roll_rate_ef angle_ef_error rateef_pitch_update_angle_and_error pitch_rate_ef angle_ef_error rateef_yaw_update_angle_and_error yaw_rate_ef angle_ef_error Vector3f rate_ef_feedforward Vector3f angle_ef_error if labs _ahrs.pitch_sensor <_acro_angle_switch _acro_angle_switch = 6000 rate_bf_targets_to_ef _rate_bf_feedforward rate_ef_feedforward rateef_roll_update_angle_and_error rate_ef_feedforward.x angle_ef_error rateef_pitch_update_angle_and_error rate_ef_feedforward.y angle_ef_error rateef_yaw_update_angle_and_error rate_ef_feedforward.z angle_ef_error rate_ef_targets_to_bf angle_ef_error _angle_bf_error else _acro_angle_switch = 4500 update_stab_rate_bf_errors rate_bf_targets_to_ef _angle_bf_error angle_ef_error _angle_ef_target.x = wrap_180_cd_float angle_ef_error.x + _ahrs.roll_sensor _angle_ef_target.y = wrap_180_cd_float angle_ef_error.y + _ahrs.pitch_sensor _angle_ef_target.z = wrap_360_cd_float angle_ef_error.z + _ahrs.yaw_sensor if _angle_ef_target.y>9000 _angle_ef_target.x = wrap_180_cd_float _angle_ef_target.x + 18000 _angle_ef_target.y = wrap_180_cd_float 18000-_angle_ef_target.x _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + 18000 if _angle_ef_target.y<-9000 _angle_ef_target.x = wrap_180_cd_float _angle_ef_target.x + 18000 _angle_ef_target.y = wrap_180_cd_float -18000-_angle_ef_target.x _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + 18000 rate_ef_target.x = rate_bf_target.x + _ahrs.sin_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * rate_bf_target.y + _ahrs.cos_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * rate_bf_target.z void AC_AttitudeControl::rateef_roll_update_angle_and_error float roll_rate_ef Vector3f &angle_ef_error _angle_ef_target.x += roll_rate_ef * _dt _angle_ef_target.x = wrap_180_cd _angle_ef_target.x angle_ef_error.x = wrap_180_cd _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.x = constrain_float angle_ef_error.x -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX _angle_ef_target.x = wrap_180_cd angle_ef_error.x + _ahrs.roll_sensor void AC_AttitudeControl::rateef_pitch_update_angle_and_error float pitch_rate_ef Vector3f &angle_ef_error _angle_ef_target.y += pitch_rate_ef * _dt _angle_ef_target.y = wrap_180_cd _angle_ef_target.y angle_ef_error.y = wrap_180_cd _angle_ef_target.y - _ahrs.pitch_sensor angle_ef_error.y = constrain_float angle_ef_error.y -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX _angle_ef_target.y = wrap_180_cd angle_ef_error.y + _ahrs.pitch_sensor void AC_AttitudeControl::rateef_yaw_update_angle_and_error float yaw_rate_ef Vector3f &angle_ef_error _angle_ef_target.z += yaw_rate_ef * _dt _angle_ef_target.z = wrap_360_cd _angle_ef_target.z angle_ef_error.z = wrap_180_cd _angle_ef_target.z - _ahrs.yaw_sensor angle_ef_error.z = constrain_float angle_ef_error.z -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX _angle_ef_target.z = wrap_360_cd angle_ef_error.z + _ahrs.yaw_sensor "
ArduPilot 190 AC_AttitudeControl.cpp Non-Control a6ad51b38f AC_AttControl: rename most methods " void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw float roll_angle_ef float pitch_angle_ef float yaw_rate_ef update_ef_yaw_angle_and_error yaw_rate_ef angle_ef_error frame_conversion_ef_to_bf angle_ef_error _angle_bf_error frame_conversion_ef_to_bf rate_ef_feedforward _rate_bf_feedforward update_rate_bf_targets void AC_AttitudeControl::angle_ef_roll_pitch_yaw float roll_angle_ef float pitch_angle_ef float yaw_angle_ef bool slew_yaw frame_conversion_ef_to_bf angle_ef_error _angle_bf_error update_rate_bf_targets void AC_AttitudeControl::rate_ef_roll_pitch_yaw float roll_rate_ef float pitch_rate_ef float yaw_rate_ef update_ef_roll_angle_and_error roll_rate_ef angle_ef_error update_ef_pitch_angle_and_error pitch_rate_ef angle_ef_error update_ef_yaw_angle_and_error yaw_rate_ef angle_ef_error frame_conversion_ef_to_bf angle_ef_error _angle_bf_error frame_conversion_ef_to_bf rate_ef_feedforward _rate_bf_feedforward update_rate_bf_targets void AC_AttitudeControl::rate_bf_roll_pitch_yaw float roll_rate_bf float pitch_rate_bf float yaw_rate_bf frame_conversion_bf_to_ef _rate_bf_feedforward rate_ef_feedforward update_ef_roll_angle_and_error rate_ef_feedforward.x angle_ef_error update_ef_pitch_angle_and_error rate_ef_feedforward.y angle_ef_error update_ef_yaw_angle_and_error rate_ef_feedforward.z angle_ef_error frame_conversion_ef_to_bf angle_ef_error _angle_bf_error integrate_bf_rate_error_to_angle_errors frame_conversion_bf_to_ef _angle_bf_error angle_ef_error update_rate_bf_targets void AC_AttitudeControl::frame_conversion_ef_to_bf const Vector3f& ef_vector Vector3f& bf_vector bf_vector.x = ef_vector.x - _ahrs.sin_pitch * ef_vector.z bf_vector.y = _ahrs.cos_roll * ef_vector.y + _ahrs.sin_roll * _ahrs.cos_pitch * ef_vector.z bf_vector.z = -_ahrs.sin_roll * ef_vector.y + _ahrs.cos_pitch * _ahrs.cos_roll * ef_vector.z void AC_AttitudeControl::frame_conversion_bf_to_ef const Vector3f& bf_vector Vector3f& ef_vector ef_vector.x = bf_vector.x + _ahrs.sin_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * bf_vector.y + _ahrs.cos_roll * _ahrs.sin_pitch /_ahrs.cos_pitch * bf_vector.z ef_vector.y = _ahrs.cos_roll * bf_vector.y - _ahrs.sin_roll * bf_vector.z ef_vector.z = _ahrs.sin_roll / _ahrs.cos_pitch * bf_vector.y + _ahrs.cos_roll / _ahrs.cos_pitch * bf_vector.z void AC_AttitudeControl::update_ef_roll_angle_and_error float roll_rate_ef Vector3f &angle_ef_error void AC_AttitudeControl::update_ef_pitch_angle_and_error float pitch_rate_ef Vector3f &angle_ef_error void AC_AttitudeControl::update_ef_yaw_angle_and_error float yaw_rate_ef Vector3f &angle_ef_error void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors void AC_AttitudeControl::update_rate_bf_targets "
ArduPilot 191 AC_AttitudeControl.cpp Control ff9f4fe6e7 AC_AttControl: replace APM_PI with AC_P " _rate_bf_target.x = _p_angle_roll.kP * _angle_bf_error.x _rate_bf_target.y = _p_angle_pitch.kP * _angle_bf_error.y _rate_bf_target.z = _p_angle_yaw.kP * _angle_bf_error.z "
ArduPilot 192 AC_AttitudeControl.cpp Control b8d9bdb794 AC_AttControl: accel limit for roll pitch yaw rates " AP_GROUPINFO ACCEL_RP_MAX 3 AC_AttitudeControl _accel_rp_max AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT AP_GROUPINFO ACCEL_Y_MAX 4 AC_AttitudeControl _accel_y_max AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT _angle_bf_error.zero const Vector3f& gyro = _ins.get_gyro _rate_bf_desired = gyro * AC_ATTITUDE_CONTROL_DEGX100 frame_conversion_bf_to_ef _rate_bf_desired _rate_ef_desired float rate_change_limit = _accel_y_max * _dt float rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired _rate_bf_target += _rate_bf_desired Vector3f angle_ef_error float rate_change_limit = _accel_rp_max * _dt float rate_change = roll_rate_ef - _rate_ef_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.x += rate_change rate_change = pitch_rate_ef - _rate_ef_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.y += rate_change rate_change_limit = _accel_y_max * _dt rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired _rate_bf_target += _rate_bf_desired frame_conversion_bf_to_ef _rate_bf_desired _rate_ef_desired update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error float rate_change rate_change_limit rate_change_limit = _accel_rp_max * _dt rate_change = roll_rate_bf - _rate_bf_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.x += rate_change rate_change = pitch_rate_bf - _rate_bf_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.y += rate_change rate_change_limit = _accel_y_max * _dt rate_change = yaw_rate_bf - _rate_bf_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.z += rate_change _rate_bf_target += _rate_bf_desired _angle_bf_error.x += _rate_bf_desired.x - _ins.get_gyro .x * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.y += _rate_bf_desired.y - _ins.get_gyro .y * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.z += _rate_bf_desired.z - _ins.get_gyro .z * AC_ATTITUDE_CONTROL_DEGX100 * _dt "
ArduPilot 193 AC_AttitudeControl.cpp Non-Control d277b6cabd AC_AttControl: fix @Units parameter descriptions ""
ArduPilot 194 AC_AttitudeControl.cpp Control 15f88c2a2b AC_AttControl: Stabilize input shaping " void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth float roll_angle_ef float pitch_angle_ef float yaw_rate_ef Vector3f angle_ef_error float rate_change_limit float rc_feel_rp = 0.1 float pid_P = rc_feel_rp/_dt float linear_angle = _accel_rp_max/ 2.0*pid_P*pid_P rate_change_limit = _accel_rp_max * _dt float rate_ef_desired float angle_to_target angle_to_target = roll_angle_ef - _angle_ef_target.x if angle_to_target > 2.0*linear_angle rate_ef_desired = safe_sqrt 2.0*_accel_rp_max* fabs angle_to_target -linear_angle else if angle_to_target < -2.0*linear_angle rate_ef_desired = -safe_sqrt 2.0*_accel_rp_max* fabs angle_to_target -linear_angle else rate_ef_desired = pid_P*angle_to_target _rate_ef_desired.x = constrain_float rate_ef_desired _rate_ef_desired.x-rate_change_limit _rate_ef_desired.x+rate_change_limit _rate_ef_desired.x = constrain_float _rate_ef_desired.x -_angle_rate_rp_max _angle_rate_rp_max _angle_ef_target.x += _rate_ef_desired.x*_dt angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor angle_to_target = pitch_angle_ef - _angle_ef_target.y if angle_to_target > 2.0*linear_angle rate_ef_desired = safe_sqrt 2.0*_accel_rp_max* fabs angle_to_target -linear_angle else if angle_to_target < -2.0*linear_angle rate_ef_desired = -safe_sqrt 2.0*_accel_rp_max* fabs angle_to_target -linear_angle else rate_ef_desired = pid_P*angle_to_target _rate_ef_desired.y = constrain_float rate_ef_desired _rate_ef_desired.y-rate_change_limit _rate_ef_desired.y+rate_change_limit _rate_ef_desired.y = constrain_float _rate_ef_desired.y -_angle_rate_rp_max _angle_rate_rp_max _angle_ef_target.y += _rate_ef_desired.y*_dt angle_ef_error.y = wrap_180_cd_float _angle_ef_target.y - _ahrs.pitch_sensor rate_change_limit = _accel_y_max * _dt float rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error frame_conversion_ef_to_bf angle_ef_error _angle_bf_error frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired update_rate_bf_targets _rate_bf_target += _rate_bf_desired "
ArduPilot 195 AC_AttitudeControl.cpp Control 25ee5d5dc8 AC_AttControl: smoothing_gain to angle_ef_roll_pitch_rate_ef_yaw_smoothpair-programmed with lthall " void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth float roll_angle_ef float pitch_angle_ef float yaw_rate_ef float smoothing_gain Vector3f angle_ef_error smoothing_gain = constrain_float smoothing_gain 1.0f 50.0f float linear_angle = _accel_rp_max/ smoothing_gain*smoothing_gain angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.y = wrap_180_cd_float _angle_ef_target.y - _ahrs.pitch_sensor if angle_to_target > linear_angle rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else if angle_to_target < -linear_angle rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f rate_ef_desired = smoothing_gain*angle_to_target if angle_to_target > linear_angle rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else if angle_to_target < -linear_angle rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f rate_ef_desired = smoothing_gain*angle_to_target _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor _angle_ef_target.x += roll_rate_ef * _dt _angle_ef_target.x = wrap_180_cd _angle_ef_target.x _angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor _angle_ef_target.y += pitch_rate_ef * _dt _angle_ef_target.y = wrap_180_cd _angle_ef_target.y _angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor _angle_ef_target.z += yaw_rate_ef * _dt _angle_ef_target.z = wrap_360_cd _angle_ef_target.z "
ArduPilot 196 AC_AttitudeControl.cpp Control 7bff8e9312 AC_AttControl: constrain earth frame yaw angle errorThis temporarily resolves the wobble we see in Auto when the targetheading is suddenly changed leading to a large yaw angle error " if slew_yaw angle_ef_error.z = constrain_float angle_ef_error.z -_slew_yaw _slew_yaw "
ArduPilot 197 AC_AttitudeControl.cpp Non-Control 5876a2fe47 AC_AttControl: increase default rp accel to 900deg/s/s ""
ArduPilot 198 AC_AttitudeControl.cpp Control 0969e464fb AC_AttControl: set roll pitch yaw rate control D term filters " void AC_AttitudeControl::set_dt float delta_sec _dt = delta_sec _pid_rate_roll.set_d_lpf_alpha AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER _dt _pid_rate_pitch.set_d_lpf_alpha AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER _dt _pid_rate_yaw.set_d_lpf_alpha AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER _dt "
ArduPilot 199 AC_AttitudeControl.cpp Control 1bdde31f6b Copter: Fix zero throttle flip issue StabilizeThe problem is that the init_targets is intended to ensure thecontroller doesn t build up the I term when the throttle is low. Becauseit is poorly named (or used) it have been written to do something else.Here I change it to do what the code is trying to use it to do. " _rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100 "
ArduPilot 200 AC_AttitudeControl.cpp Non-Control 5209598459 AC_AttControl: rename init_targets to relax_bf_rate_controller " void AC_AttitudeControl::relax_bf_rate_controller "
ArduPilot 201 AC_AttitudeControl.cpp Control 922026c15c AC_AttControl: rate compensation for yaw " _rate_bf_target.x += -_angle_bf_error.y * _ins.get_gyro .z _rate_bf_target.y += _angle_bf_error.x * _ins.get_gyro .z "
ArduPilot 202 AC_AttitudeControl.cpp Control c24d293e1b AC_AttControl: zero _accel_xyz_max stops feed forwardAlso added place holder for turning off feed forward. " if _accel_rp_max > 0 angle_to_target = roll_angle_ef - _angle_ef_target.x if angle_to_target > linear_angle rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else if angle_to_target < -linear_angle rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else rate_ef_desired = smoothing_gain*angle_to_target _rate_ef_desired.x = constrain_float rate_ef_desired _rate_ef_desired.x-rate_change_limit _rate_ef_desired.x+rate_change_limit _angle_ef_target.x += _rate_ef_desired.x*_dt angle_to_target = pitch_angle_ef - _angle_ef_target.y if angle_to_target > linear_angle rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else if angle_to_target < -linear_angle rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* fabs angle_to_target - linear_angle/2.0f else rate_ef_desired = smoothing_gain*angle_to_target _rate_ef_desired.y = constrain_float rate_ef_desired _rate_ef_desired.y-rate_change_limit _rate_ef_desired.y+rate_change_limit _angle_ef_target.y += _rate_ef_desired.y*_dt _angle_ef_target.x = roll_angle_ef _angle_ef_target.y = pitch_angle_ef _rate_ef_desired.x = 0 _rate_ef_desired.y = 0 if _accel_y_max > 0 rate_change_limit = _accel_y_max * _dt float rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error _rate_ef_desired.z = 0 update_ef_yaw_angle_and_error yaw_rate_ef angle_ef_error if _accel_rp_max > 0 rate_change_limit = _accel_rp_max * _dt rate_change = roll_rate_bf - _rate_bf_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.x += rate_change rate_change = pitch_rate_bf - _rate_bf_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.y += rate_change else _rate_bf_desired.x = roll_rate_bf _rate_bf_desired.y = pitch_rate_bf if _accel_y_max > 0 rate_change_limit = _accel_y_max * _dt rate_change = yaw_rate_bf - _rate_bf_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.z += rate_change else _rate_bf_desired.z = yaw_rate_bf "
ArduPilot 203 AC_AttitudeControl.cpp Non-Control 2bb63857fa AC_AttControl: clean up stabilize " update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor angle_ef_error.y = wrap_180_cd_float _angle_ef_target.y - _ahrs.pitch_sensor "
ArduPilot 204 AC_AttitudeControl.cpp Control 698cf934b8 AC_AttControl: formatting fixes " if _accel_rp_max > 0 if angle_to_target > linear_angle else if angle_to_target < -linear_angle if angle_to_target > linear_angle else if angle_to_target < -linear_angle if _accel_y_max > 0 if labs _ahrs.pitch_sensor <_acro_angle_switch if _angle_ef_target.y > 9000.0f _angle_ef_target.x = wrap_180_cd_float _angle_ef_target.x + 18000.0f _angle_ef_target.y = wrap_180_cd_float 18000.0f - _angle_ef_target.x _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + 18000.0f if _angle_ef_target.y < -9000.0f _angle_ef_target.x = wrap_180_cd_float _angle_ef_target.x + 18000.0f _angle_ef_target.y = wrap_180_cd_float -18000.0f - _angle_ef_target.x _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + 18000.0f "
ArduPilot 205 AC_AttitudeControl.cpp Control 7f734f38d6 AC_AttControl: add earth frame angle constraintsThis should help recovery time if pilot switches out of ACRO (intoStabilize AltHold etc) while inverted " _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float _angle_ef_target.y -_aparm.angle_max _aparm.angle_max _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float _angle_ef_target.y -_aparm.angle_max _aparm.angle_max _angle_ef_target.x = constrain_float roll_angle_ef -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float pitch_angle_ef -_aparm.angle_max _aparm.angle_max _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float _angle_ef_target.y -_aparm.angle_max _aparm.angle_max "
ArduPilot 206 AC_AttitudeControl.cpp Non-Control a662f87ffb AC_AttControl: remove resolved To-DoThis To-Do is resolved by heli flight modes calling therelax_bf_rate_controller method ""
ArduPilot 207 AC_AttitudeControl.cpp Control d9c966c927 AC_AttControl: RATE_FF_ENAB param to disable rate feed forward " AP_GROUPINFO RATE_FF_ENAB 5 AC_AttitudeControl _rate_bf_ff_enabled AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT if _rate_bf_ff_enabled if _rate_bf_ff_enabled if _rate_bf_ff_enabled _rate_bf_target += _rate_bf_desired if _rate_bf_ff_enabled _rate_bf_target += _rate_bf_desired if _rate_bf_ff_enabled _rate_bf_target += _rate_bf_desired if _rate_bf_ff_enabled if _rate_bf_ff_enabled if _rate_bf_ff_enabled _rate_bf_target += _rate_bf_desired "
ArduPilot 208 AC_AttitudeControl.cpp Non-Control b57c0dabf6 AC_AttControl: check accel_rp_max instead of rate_bff_ff_enabledWe use the accel_rp_max accel_y_max to check whether to apply accellimiting or not. This is related to separate from thebody-frame-feed-forward. " if _accel_rp_max > 0.0f if _accel_y_max > 0.0f _rate_bf_target.z += _rate_bf_desired.z if _accel_rp_max > 0.0f if _accel_y_max > 0.0f if _accel_rp_max > 0.0f _rate_bf_target.x += _rate_bf_desired.x _rate_bf_target.y += _rate_bf_desired.y if _accel_y_max > 0.0f _rate_bf_target.z += _rate_bf_desired.z "
ArduPilot 209 AC_AttitudeControl.cpp Control 130eb07d48 AC_AttControl: angle_ef_roll_pitch_rate_ef_yaw supports zero yaw accel " if _accel_y_max > 0.0f float rate_change_limit = _accel_y_max * _dt float rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error else _rate_ef_desired.z = 0 update_ef_yaw_angle_and_error yaw_rate_ef angle_ef_error "
ArduPilot 210 AC_AttitudeControl.cpp Control 46f25c52a4 AC_AttControl: allow enabling/disabling feedforward and accel limiting " void AC_AttitudeControl::accel_limiting bool enable_limits if enable_limits if _accel_rp_max == 0.0f _accel_rp_max.load if _accel_y_max == 0.0f _accel_y_max.load else _accel_rp_max = 0.0f _accel_y_max = 0.0f hal.console->printf_P PSTR AccLim:%d int enable_limits "
ArduPilot 211 AC_AttitudeControl.cpp Non-Control 971411e0db AC_AttControl: fixed typo in parameter description ""
ArduPilot 212 AC_AttitudeControl.cpp Control 0f7178e447 AC_AttControl: cast fabs to float to resolve compiler warning " rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* float fabs angle_to_target - linear_angle/2.0f rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* float fabs angle_to_target - linear_angle/2.0f rate_ef_desired = safe_sqrt 2.0f*_accel_rp_max* float fabs angle_to_target - linear_angle/2.0f rate_ef_desired = -safe_sqrt 2.0f*_accel_rp_max* float fabs angle_to_target - linear_angle/2.0f "
ArduPilot 213 AC_AttitudeControl.cpp Control 6e66cf43cb AC_AttitudeControl: Use bias-corrected angular rates instead of raw gyro measurements " const Vector3f& gyro = _ahrs.get_gyro _angle_bf_error.x += _rate_bf_desired.x - _ahrs.get_gyro .x * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.y += _rate_bf_desired.y - _ahrs.get_gyro .y * AC_ATTITUDE_CONTROL_DEGX100 * _dt _angle_bf_error.z += _rate_bf_desired.z - _ahrs.get_gyro .z * AC_ATTITUDE_CONTROL_DEGX100 * _dt _rate_bf_target.x += -_angle_bf_error.y * _ahrs.get_gyro .z _rate_bf_target.y += _angle_bf_error.x * _ahrs.get_gyro .z current_rate = _ahrs.get_gyro .x * AC_ATTITUDE_CONTROL_DEGX100 current_rate = _ahrs.get_gyro .y * AC_ATTITUDE_CONTROL_DEGX100 current_rate = _ahrs.get_gyro .z * AC_ATTITUDE_CONTROL_DEGX100 "
ArduPilot 214 AC_AttitudeControl.cpp Control 2889f5abc4 AC_AttControl: Fix feedforward behavior " _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float _angle_ef_target.y -_aparm.angle_max _aparm.angle_max _rate_ef_desired.z = yaw_rate_ef update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired _rate_bf_target += _rate_bf_desired else Vector3f rate_ef_desired 0 0 _rate_ef_desired.z frame_conversion_ef_to_bf rate_ef_desired _rate_bf_desired _angle_ef_target.x = constrain_float roll_angle_ef -_aparm.angle_max _aparm.angle_max _angle_ef_target.y = constrain_float pitch_angle_ef -_aparm.angle_max _aparm.angle_max _rate_ef_desired.z = yaw_rate_ef update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error _rate_ef_desired.x = 0 _rate_ef_desired.y = 0 frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired _rate_bf_target += _rate_bf_desired float rate_change_limit rate_change if _accel_rp_max > 0.0f rate_change_limit = _accel_rp_max * _dt rate_change = roll_rate_ef - _rate_ef_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.x += rate_change rate_change = pitch_rate_ef - _rate_ef_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.y += rate_change else _rate_ef_desired.x = roll_rate_ef _rate_ef_desired.y = pitch_rate_ef if _accel_y_max > 0.0f rate_change_limit = _accel_y_max * _dt rate_change = yaw_rate_ef - _rate_ef_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_ef_desired.z += rate_change else _rate_ef_desired.z = yaw_rate_ef frame_conversion_ef_to_bf _rate_ef_desired _rate_bf_desired _rate_bf_target += _rate_bf_desired _rate_bf_target += _rate_bf_desired "
ArduPilot 215 AC_AttitudeControl.cpp Control fb55658c91 Copter: Acro use same error limit in all three axis " update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX update_ef_roll_angle_and_error _rate_ef_desired.x angle_ef_error AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX update_ef_pitch_angle_and_error _rate_ef_desired.y angle_ef_error AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX update_ef_yaw_angle_and_error _rate_ef_desired.z angle_ef_error AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX void AC_AttitudeControl::update_ef_roll_angle_and_error float roll_rate_ef Vector3f &angle_ef_error float overshoot_max angle_ef_error.x = constrain_float angle_ef_error.x -overshoot_max overshoot_max void AC_AttitudeControl::update_ef_pitch_angle_and_error float pitch_rate_ef Vector3f &angle_ef_error float overshoot_max angle_ef_error.y = constrain_float angle_ef_error.y -overshoot_max overshoot_max void AC_AttitudeControl::update_ef_yaw_angle_and_error float yaw_rate_ef Vector3f &angle_ef_error float overshoot_max angle_ef_error.z = constrain_float angle_ef_error.z -overshoot_max overshoot_max _angle_bf_error.x = constrain_float _angle_bf_error.x -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX _angle_bf_error.y = constrain_float _angle_bf_error.y -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX _angle_bf_error.z = constrain_float _angle_bf_error.z -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX "
ArduPilot 216 AC_AttitudeControl.cpp Non-Control eebd32f306 AC_AttControl: remove duplicate rate_ef_desired to resolve compiler warning " frame_conversion_ef_to_bf Vector3f 0 0 _rate_ef_desired.z _rate_bf_desired "
ArduPilot 217 AC_AttitudeControl.cpp Non-Control c2f8571f37 AC_AttControl: FF and accel limiting off by defaultAlso adjust parameter ranges used by GCSs ""
ArduPilot 218 AC_AttitudeControl.cpp Control 90dc9411a5 Copter: ACRO Error calculation fix " float rate_change rate_change_limit if _accel_rp_max > 0.0f rate_change_limit = _accel_rp_max * _dt rate_change = roll_rate_bf - _rate_bf_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.x += rate_change rate_change = pitch_rate_bf - _rate_bf_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.y += rate_change else _rate_bf_desired.x = roll_rate_bf _rate_bf_desired.y = pitch_rate_bf if _accel_y_max > 0.0f rate_change_limit = _accel_y_max * _dt rate_change = yaw_rate_bf - _rate_bf_desired.z rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.z += rate_change else _rate_bf_desired.z = yaw_rate_bf "
ArduPilot 219 AC_AttitudeControl.cpp Control 329118b7c9 Copter: AC_ATT correct yaw error calculation " _rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro .z _rate_bf_target.y += -_angle_bf_error.x * _ahrs.get_gyro .z "
ArduPilot 220 AC_AttitudeControl.cpp Control 58257e3858 AC_AttControl: set rate D term filter from ins filter " const AP_InertialSensor &ins = _ahrs.get_ins float ins_filter = float ins.get_filter if ins_filter <= 0.0f ins_filter = AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER _pid_rate_roll.set_d_lpf_alpha ins_filter _dt _pid_rate_pitch.set_d_lpf_alpha ins_filter _dt _pid_rate_yaw.set_d_lpf_alpha ins_filter/2.0f _dt "
ArduPilot 221 AC_AttitudeControl.cpp Control c45338f080 AC_AttControl: div-by-zero check for bf-to-ef conversion " if frame_conversion_bf_to_ef _angle_bf_error angle_ef_error _angle_ef_target.x = wrap_180_cd_float angle_ef_error.x + _ahrs.roll_sensor _angle_ef_target.y = wrap_180_cd_float angle_ef_error.y + _ahrs.pitch_sensor _angle_ef_target.z = wrap_360_cd_float angle_ef_error.z + _ahrs.yaw_sensor bool AC_AttitudeControl::frame_conversion_bf_to_ef const Vector3f& bf_vector Vector3f& ef_vector if _ahrs.cos_pitch == 0.0f return false return true "
ArduPilot 222 AC_AttitudeControl.cpp Non-Control 3e0b573dfe AC_AttControl: remove debug message ""
ArduPilot 223 AC_AttitudeControl.cpp Control 5a66ff1ef9 AC_AttControl: bug fix for ef target during acro " _angle_ef_target.y = wrap_180_cd_float 18000.0f - _angle_ef_target.y _angle_ef_target.y = wrap_180_cd_float -18000.0f - _angle_ef_target.y "
ArduPilot 224 AC_AttitudeControl.cpp Non-Control f65e81cb07 AC_AttControl: remove some old comments ""
ArduPilot 225 AC_AttitudeControl.cpp Control fc898a00a3 AC_AttControl: add sqrt_controllerOriginal code by Jonathan Challinger " float AC_AttitudeControl::sqrt_controller float error float p float second_ord_lim if second_ord_lim == 0.0f || p == 0.0f return error*p float linear_dist = second_ord_lim/sq p if error > linear_dist return safe_sqrt 2.0f*second_ord_lim* error- linear_dist/2.0f else if error < -linear_dist return -safe_sqrt 2.0f*second_ord_lim* -error- linear_dist/2.0f else return error*p "
ArduPilot 226 AC_AttitudeControl.cpp Control 0c4a489491 AC_AttitudeControl: use sqrt_controller function " rate_ef_desired = sqrt_controller roll_angle_ef-_angle_ef_target.x smoothing_gain _accel_rp_max rate_ef_desired = sqrt_controller pitch_angle_ef-_angle_ef_target.y smoothing_gain _accel_rp_max "
ArduPilot 227 AC_AttitudeControl.cpp Control 7224669399 AC_AttitudeControl: cleanup angle_ef_roll_pitch_rate_ef_yaw_smooth " float rate_ef_desired float rate_change_limit = _accel_rp_max * _dt _rate_ef_desired.x = constrain_float rate_ef_desired _rate_ef_desired.x-rate_change_limit _rate_ef_desired.x+rate_change_limit _rate_ef_desired.y = constrain_float rate_ef_desired _rate_ef_desired.y-rate_change_limit _rate_ef_desired.y+rate_change_limit _angle_ef_target.x = roll_angle_ef _angle_ef_target.y = pitch_angle_ef _rate_ef_desired.x = 0 _rate_ef_desired.y = 0 float rate_change_limit = _accel_y_max * _dt _rate_ef_desired.z += constrain_float yaw_rate_ef - _rate_ef_desired.z -rate_change_limit rate_change_limit _rate_ef_desired.z = yaw_rate_ef "
ArduPilot 228 AC_AttitudeControl.cpp Control eb084f7c58 AC_AttControl: bf yaw control uses input filtered PID " _pid_rate_yaw.set_d_lpf_alpha ins_filter/4.0f _dt d = _pid_rate_yaw.get_d_filt rate_error _dt p = _pid_rate_yaw.get_p_filt i = _pid_rate_yaw.get_i_filt _dt "
ArduPilot 229 AC_AttitudeControl.cpp Control 9833c91b2b AC_AttControl: get_max_rate_step_bf_roll pitch and yaw " float AC_AttitudeControl::max_rate_step_bf_roll float alpha = _pid_rate_roll.get_filt_alpha float alpha_remaining = 1-alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_roll.kD /_dt + _pid_rate_roll.kP float AC_AttitudeControl::max_rate_step_bf_pitch float alpha = _pid_rate_pitch.get_filt_alpha float alpha_remaining = 1-alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_pitch.kD /_dt + _pid_rate_pitch.kP float AC_AttitudeControl::max_rate_step_bf_yaw float alpha = _pid_rate_yaw.get_filt_alpha float alpha_remaining = 1-alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_yaw.kD /_dt + _pid_rate_yaw.kP "
ArduPilot 230 AC_AttitudeControl.cpp Control 792b2a2eb3 AC_AttControl: separate accel max for roll pitch yawAlso add:Rate filtersrename rate filter definesd-term only filter for roll pitch rate controlaccessors to save max accel for roll pitch yawfix for duplicate ACCEL_Y_MAX param " AP_GROUPINFO ACCEL_Y_MAX 4 AC_AttitudeControl _accel_yaw_max AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT AP_GROUPINFO ACCEL_R_MAX 6 AC_AttitudeControl _accel_roll_max AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT AP_GROUPINFO ACCEL_P_MAX 7 AC_AttitudeControl _accel_pitch_max AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT _pid_rate_roll.set_dt _dt _pid_rate_pitch.set_dt _dt _pid_rate_yaw.set_dt _dt float rate_ef_desired float rate_change_limit if _accel_roll_max > 0.0f rate_change_limit = _accel_roll_max * _dt rate_ef_desired = sqrt_controller roll_angle_ef-_angle_ef_target.x smoothing_gain _accel_roll_max else _angle_ef_target.x = roll_angle_ef angle_ef_error.x = wrap_180_cd_float _angle_ef_target.x - _ahrs.roll_sensor _rate_ef_desired.x = 0 _angle_ef_target.x = constrain_float _angle_ef_target.x -_aparm.angle_max _aparm.angle_max if _accel_pitch_max > 0.0f rate_change_limit = _accel_pitch_max * _dt rate_ef_desired = sqrt_controller pitch_angle_ef-_angle_ef_target.y smoothing_gain _accel_pitch_max if _accel_yaw_max > 0.0f rate_change_limit = _accel_yaw_max * _dt if _accel_yaw_max > 0.0f float rate_change_limit = _accel_yaw_max * _dt if _accel_roll_max > 0.0f rate_change_limit = _accel_roll_max * _dt else _rate_ef_desired.x = roll_rate_ef if _accel_pitch_max > 0.0f rate_change_limit = _accel_pitch_max * _dt if _accel_yaw_max > 0.0f rate_change_limit = _accel_yaw_max * _dt if _accel_roll_max > 0.0f rate_change_limit = _accel_roll_max * _dt rate_change = roll_rate_bf - _rate_bf_desired.x rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.x += rate_change else _rate_bf_desired.x = roll_rate_bf if _accel_pitch_max > 0.0f rate_change_limit = _accel_pitch_max * _dt rate_change = pitch_rate_bf - _rate_bf_desired.y rate_change = constrain_float rate_change -rate_change_limit rate_change_limit _rate_bf_desired.y += rate_change _rate_bf_desired.y = pitch_rate_bf if _accel_yaw_max > 0.0f rate_change_limit = _accel_yaw_max * _dt _pid_rate_roll.set_input_filter_d rate_error p = _pid_rate_roll.get_p i = _pid_rate_roll.get_i d = _pid_rate_roll.get_d _pid_rate_pitch.set_input_filter_d rate_error p = _pid_rate_pitch.get_p i = _pid_rate_pitch.get_i d = _pid_rate_pitch.get_d _pid_rate_yaw.set_input_filter_all rate_error p = _pid_rate_yaw.get_p i = _pid_rate_yaw.get_i d = _pid_rate_yaw.get_d if _accel_roll_max == 0.0f _accel_roll_max.load if _accel_pitch_max == 0.0f _accel_pitch_max.load if _accel_yaw_max == 0.0f _accel_yaw_max.load _accel_roll_max = 0.0f _accel_pitch_max = 0.0f _accel_yaw_max = 0.0f "
ArduPilot 231 AC_AttitudeControl.cpp Control 20de383084 AC_AttControl: accel limiting for angular control only if feed forward enabled " if _accel_roll_max > 0.0f && _rate_bf_ff_enabled if _accel_pitch_max > 0.0f && _rate_bf_ff_enabled "
ArduPilot 232 AC_AttitudeControl.cpp Control e2383581cc AC_AttitudeControl: relax_bf_rate_controller resets rate integrators " _pid_rate_roll.reset_I _pid_rate_pitch.reset_I _pid_rate_yaw.reset_I "
ArduPilot 233 AC_AttitudeControl.cpp Control afcd1c6ec3 AC_AttitudeControl: sqrt controller on Stab " _rate_bf_target.x = sqrt_controller _angle_bf_error.x _p_angle_roll.kP constrain_float _accel_roll_max/2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX else _rate_bf_target.x = _p_angle_roll.kP * _angle_bf_error.x _rate_bf_target.y = sqrt_controller _angle_bf_error.y _p_angle_pitch.kP constrain_float _accel_pitch_max/2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX else _rate_bf_target.y = _p_angle_pitch.kP * _angle_bf_error.y _rate_bf_target.z = sqrt_controller _angle_bf_error.z _p_angle_yaw.kP constrain_float _accel_yaw_max/2.0f AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX else _rate_bf_target.z = _p_angle_yaw.kP * _angle_bf_error.z "
ArduPilot 234 AC_AttitudeControl.cpp Control 5c2341009a AC_AttitudeControl: throttle is a float " void AC_AttitudeControl::set_throttle_out float throttle_out bool apply_angle_boost float AC_AttitudeControl::get_angle_boost float throttle_pwm float throttle_out "
ArduPilot 235 AC_AttitudeControl.cpp Control f3555d0d43 AC_AttitudeControl: modify throttle interface to specify stabilization " _motors.set_stabilizing true void AC_AttitudeControl::set_throttle_out_unstabilized float throttle_in bool reset_attitude_control if reset_attitude_control relax_bf_rate_controller set_yaw_target_to_current_heading _motors.set_stabilizing false _motors.set_throttle throttle_in _angle_boost = 0 "
ArduPilot 236 AC_AttitudeControl.cpp Control 518e798f53 AC_AttitudeControl: rework angle_boost " _motors.set_throttle get_boosted_throttle throttle_out float AC_AttitudeControl::get_boosted_throttle float throttle_in float min_throttle = _motors.throttle_min float cos_tilt = _ahrs.cos_pitch * _ahrs.cos_roll float inverted_factor = constrain_float 2.0f*cos_tilt 0.0f 1.0f float boost_factor = 1.0f/constrain_float cos_tilt 0.5f 1.0f float throttle_out = throttle_in-min_throttle *inverted_factor*boost_factor + min_throttle _angle_boost = throttle_out - throttle_in "
ArduPilot 237 AC_AttitudeControl.cpp Control b7f7624aac AC_AttitudeControl: add throttle filter cutoff parameter to set_throttle_out functions " void AC_AttitudeControl::set_throttle_out float throttle_out bool apply_angle_boost float filter_cutoff _motors.set_throttle_filter_cutoff filter_cutoff void AC_AttitudeControl::set_throttle_out_unstabilized float throttle_in bool reset_attitude_control float filter_cutoff _motors.set_throttle_filter_cutoff filter_cutoff "
ArduPilot 238 AC_AttitudeControl.cpp Non-Control 24b8d32b0e AC_AttitudeControl: add comment ""
ArduPilot 239 AC_AttitudeControl.cpp Control ab2532a609 AC_AttitudeControl: constrain _angle_boost to avoid overflow " _angle_boost = constrain_float throttle_out - throttle_in -32000 32000 "
ArduPilot 240 AC_AttitudeControl.cpp Control 1bca81eaed AC_AttitudeControl: compiler warnings: apply is_zero(float) or is_equal(float) and float to doubles " #include if AP_Math::is_zero _ahrs.cos_pitch if AP_Math::is_zero _accel_roll_max if AP_Math::is_zero _accel_pitch_max if AP_Math::is_zero _accel_yaw_max if AP_Math::is_zero second_ord_lim || AP_Math::is_zero p "
ArduPilot 241 AC_AttitudeControl.cpp Control 326b0b33ea AC_AttitudeControl: revert AP_Math class change " if is_zero _ahrs.cos_pitch if is_zero _accel_roll_max if is_zero _accel_pitch_max if is_zero _accel_yaw_max if is_zero second_ord_lim || is_zero p "
ArduPilot 242 AC_AttitudeControl.cpp Non-Control 915236f1f5 AC_AttitudeControl: Non-functional comment changeDon t encourage future misuse of g.rc3 ""
ArduPilot 243 AC_AttitudeControl.cpp Non-Control 23adf2773c AC_AttitudeControl: Remove To-Do comments for Attitude Rate logging. ""
ArduPilot 244 AC_AttitudeControl.cpp Control 521dae1c65 AC_AttitudeControl: use set_desired_rate() on PID controllersthis sets them up for logging of PIDs " _pid_rate_roll.set_desired_rate rate_target_cds _pid_rate_pitch.set_desired_rate rate_target_cds _pid_rate_yaw.set_desired_rate rate_target_cds "
ArduPilot 245 AC_AttitudeControl.cpp Control 1e3e65e443 AC_AttControl: add shift_ef_yaw_targetThis allows shifting the target heading in case the EKF shifts it sheading estimate " void AC_AttitudeControl::shift_ef_yaw_target float yaw_shift_cd _angle_ef_target.z = wrap_360_cd_float _angle_ef_target.z + yaw_shift_cd "
ArduPilot 246 AC_AttitudeControl.cpp Non-Control 93dd7dd970 AC_AttControl: get_boosted_throttle purely virtual ""
ArduPilot 247 AC_AttitudeControl.cpp Control 0bf1d04172 AC_AttControl: relax earth frame rate targets along with bf " frame_conversion_bf_to_ef _rate_bf_target _rate_ef_desired "
ArduPilot 248 AC_AttitudeControl.cpp Non-Control d49f10d2e5 AC_AttitudeControl: standardize inclusion of libaries headersThis commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to so the notation #include is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source then we use the notation #include <> with the path relative to libraries folder.Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time. " #include #include "
ArduPilot 249 AC_AttitudeControl.cpp Control 29ff5035b4 AC_AttControl: limit lean angle from throttle " _throttle_in_filt.apply throttle_out _dt _throttle_in_filt.apply throttle_in _dt "
ArduPilot 250 AC_AttitudeControl.cpp Non-Control c8872e082d AC_AttControl: rename set_throttle_out parameterNo functional change " void AC_AttitudeControl::set_throttle_out float throttle_in bool apply_angle_boost float filter_cutoff _throttle_in_filt.apply throttle_in _dt _motors.set_throttle get_boosted_throttle throttle_in _motors.set_throttle throttle_in "
ArduPilot 251 AC_AttitudeControl.cpp Non-Control 831d8acca5 Remove use of PROGMEMNow variables don t have to be declared with PROGMEM anymore so removethem. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i s/ PROGMEM / /g git grep -l -z PROGMEM | xargs -0 sed -i s/PROGMEM//g The 2 commands were done so we don t leave behind spurious spaces.AVR-specific places were not changed. " const AP_Param::GroupInfo AC_AttitudeControl::var_info = "
ArduPilot 252 AC_AttitudeControl.cpp Non-Control 6784fd8625 AC_AttitudeControl: reserve parameter IDs ""
ArduPilot 253 AC_AttitudeControl.cpp Control 0b33ef3862 AC_AttitudeControl: Add Hover Roll Trim functionality for helicopters. " roll_angle_ef += get_roll_trim roll_angle_ef += get_roll_trim roll_angle_ef += get_roll_trim "
ArduPilot 254 AC_AttitudeControl.cpp Non-Control 4b41710261 AC_AttControl: minor comment fix ""
ArduPilot 255 AC_AttitudeControl.cpp Control 58f0abaf4c AC_AttitudeControl: fixed one usage of zero accel limitsin other places zero accel limit means no limit " if _flags.limit_angle_to_rate_request && _accel_roll_max > 0.0f if _flags.limit_angle_to_rate_request && _accel_pitch_max > 0.0f if _flags.limit_angle_to_rate_request && _accel_yaw_max > 0.0f "
ArduPilot 256 AC_AttitudeControl.cpp Control 7330de86e5 AC_AttitudeControl: change internals to use radians instead of centidegrees " AP_GROUPINFO SLEW_YAW 2 AC_AttitudeControl _slew_yaw AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS AP_GROUPINFO ACCEL_Y_MAX 4 AC_AttitudeControl _accel_yaw_max AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS AP_GROUPINFO ACCEL_R_MAX 6 AC_AttitudeControl _accel_roll_max AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS AP_GROUPINFO ACCEL_P_MAX 7 AC_AttitudeControl _accel_pitch_max AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS _rate_bf_target_rads = _ahrs.get_gyro frame_conversion_bf_to_ef _rate_bf_target_rads _rate_ef_desired_rads _angle_ef_target_rad.z = wrap_2PI _angle_ef_target_rad.z + radians yaw_shift_cd*0.01f void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth float roll_angle_ef_cd float pitch_angle_ef_cd float yaw_rate_ef_cds float smoothing_gain float roll_angle_ef_rad = radians roll_angle_ef_cd*0.01f float pitch_angle_ef_rad = radians pitch_angle_ef_cd*0.01f float yaw_rate_ef_rads = radians yaw_rate_ef_cds*0.01f float rate_ef_desired_rads float rate_change_limit_rads Vector3f angle_ef_error_rad roll_angle_ef_rad += get_roll_trim_rad if get_accel_roll_max_radss > 0.0f && _rate_bf_ff_enabled rate_change_limit_rads = get_accel_roll_max_radss * _dt rate_ef_desired_rads = sqrt_controller roll_angle_ef_rad-_angle_ef_target_rad.x smoothing_gain get_accel_roll_max_radss _rate_ef_desired_rads.x = constrain_float rate_ef_desired_rads _rate_ef_desired_rads.x-rate_change_limit_rads _rate_ef_desired_rads.x+rate_change_limit_rads update_ef_roll_angle_and_error _rate_ef_desired_rads.x angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _angle_ef_target_rad.x = roll_angle_ef_rad angle_ef_error_rad.x = wrap_180_cd_float _angle_ef_target_rad.x - _ahrs.roll _rate_ef_desired_rads.x = 0 _angle_ef_target_rad.x = constrain_float _angle_ef_target_rad.x -get_tilt_limit_rad get_tilt_limit_rad if get_accel_pitch_max_radss > 0.0f && _rate_bf_ff_enabled rate_change_limit_rads = get_accel_pitch_max_radss * _dt rate_ef_desired_rads = sqrt_controller pitch_angle_ef_rad-_angle_ef_target_rad.y smoothing_gain get_accel_pitch_max_radss _rate_ef_desired_rads.y = constrain_float rate_ef_desired_rads _rate_ef_desired_rads.y-rate_change_limit_rads _rate_ef_desired_rads.y+rate_change_limit_rads update_ef_pitch_angle_and_error _rate_ef_desired_rads.y angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _angle_ef_target_rad.y = pitch_angle_ef_rad angle_ef_error_rad.y = wrap_180_cd_float _angle_ef_target_rad.y - _ahrs.pitch _rate_ef_desired_rads.y = 0 _angle_ef_target_rad.y = constrain_float _angle_ef_target_rad.y -get_tilt_limit_rad get_tilt_limit_rad if get_accel_yaw_max_radss > 0.0f rate_change_limit_rads = get_accel_yaw_max_radss * _dt _rate_ef_desired_rads.z += constrain_float yaw_rate_ef_rads - _rate_ef_desired_rads.z -rate_change_limit_rads rate_change_limit_rads update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _rate_ef_desired_rads.z = yaw_rate_ef_rads update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD frame_conversion_ef_to_bf angle_ef_error_rad _angle_bf_error_rad frame_conversion_ef_to_bf _rate_ef_desired_rads _rate_bf_desired_rads _rate_bf_target_rads += _rate_bf_desired_rads frame_conversion_ef_to_bf Vector3f 0 0 _rate_ef_desired_rads.z _rate_bf_desired_rads _rate_bf_target_rads += _rate_bf_desired_rads void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw float roll_angle_ef_cd float pitch_angle_ef_cd float yaw_rate_ef_cds float roll_angle_ef_rad = radians roll_angle_ef_cd*0.01f float pitch_angle_ef_rad = radians pitch_angle_ef_cd*0.01f float yaw_rate_ef_rads = radians yaw_rate_ef_cds*0.01f Vector3f angle_ef_error_rad roll_angle_ef_rad += get_roll_trim_rad _angle_ef_target_rad.x = constrain_float roll_angle_ef_rad -get_tilt_limit_rad get_tilt_limit_rad angle_ef_error_rad.x = wrap_PI _angle_ef_target_rad.x - _ahrs.roll _angle_ef_target_rad.y = constrain_float pitch_angle_ef_rad -get_tilt_limit_rad get_tilt_limit_rad angle_ef_error_rad.y = wrap_PI _angle_ef_target_rad.y - _ahrs.pitch if get_accel_yaw_max_radss > 0.0f float rate_change_limit_rads = get_accel_yaw_max_radss * _dt float rate_change_rads = yaw_rate_ef_rads - _rate_ef_desired_rads.z rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_ef_desired_rads.z += rate_change_rads update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _rate_ef_desired_rads.z = yaw_rate_ef_rads update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD frame_conversion_ef_to_bf angle_ef_error_rad _angle_bf_error_rad _rate_ef_desired_rads.x = 0 _rate_ef_desired_rads.y = 0 frame_conversion_ef_to_bf _rate_ef_desired_rads _rate_bf_desired_rads _rate_bf_target_rads += _rate_bf_desired_rads void AC_AttitudeControl::angle_ef_roll_pitch_yaw float roll_angle_ef_cd float pitch_angle_ef_cd float yaw_angle_ef_cd bool slew_yaw float roll_angle_ef_rad = radians roll_angle_ef_cd*0.01f float pitch_angle_ef_rad = radians pitch_angle_ef_cd*0.01f float yaw_angle_ef_rad = radians yaw_angle_ef_cd*0.01f Vector3f angle_ef_error_rad roll_angle_ef_rad += get_roll_trim_rad _angle_ef_target_rad.x = constrain_float roll_angle_ef_rad -get_tilt_limit_rad get_tilt_limit_rad _angle_ef_target_rad.y = constrain_float pitch_angle_ef_rad -get_tilt_limit_rad get_tilt_limit_rad _angle_ef_target_rad.z = yaw_angle_ef_rad angle_ef_error_rad.x = wrap_PI _angle_ef_target_rad.x - _ahrs.roll angle_ef_error_rad.y = wrap_PI _angle_ef_target_rad.y - _ahrs.pitch angle_ef_error_rad.z = wrap_PI _angle_ef_target_rad.z - _ahrs.yaw angle_ef_error_rad.z = constrain_float angle_ef_error_rad.z -get_slew_yaw_rads get_slew_yaw_rads frame_conversion_ef_to_bf angle_ef_error_rad _angle_bf_error_rad void AC_AttitudeControl::rate_ef_roll_pitch_yaw float roll_rate_ef_cds float pitch_rate_ef_cds float yaw_rate_ef_cds float roll_rate_ef_rads = radians roll_rate_ef_cds*0.01f float pitch_rate_ef_rads = radians pitch_rate_ef_cds*0.01f float yaw_rate_ef_rads = radians yaw_rate_ef_cds*0.01f Vector3f angle_ef_error_rad float rate_change_limit_rads rate_change_rads if get_accel_roll_max_radss > 0.0f rate_change_limit_rads = get_accel_roll_max_radss * _dt rate_change_rads = roll_rate_ef_rads - _rate_ef_desired_rads.x rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_ef_desired_rads.x += rate_change_rads _rate_ef_desired_rads.x = roll_rate_ef_rads if get_accel_pitch_max_radss > 0.0f rate_change_limit_rads = get_accel_pitch_max_radss * _dt rate_change_rads = pitch_rate_ef_rads - _rate_ef_desired_rads.y rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_ef_desired_rads.y += rate_change_rads _rate_ef_desired_rads.y = pitch_rate_ef_rads if get_accel_yaw_max_radss > 0.0f rate_change_limit_rads = get_accel_yaw_max_radss * _dt rate_change_rads = yaw_rate_ef_rads - _rate_ef_desired_rads.z rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_ef_desired_rads.z += rate_change_rads _rate_ef_desired_rads.z = yaw_rate_ef_rads update_ef_roll_angle_and_error _rate_ef_desired_rads.x angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_ef_pitch_angle_and_error _rate_ef_desired_rads.y angle_ef_error_rad AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _angle_ef_target_rad.x = constrain_float _angle_ef_target_rad.x -get_tilt_limit_rad get_tilt_limit_rad _angle_ef_target_rad.y = constrain_float _angle_ef_target_rad.y -get_tilt_limit_rad get_tilt_limit_rad frame_conversion_ef_to_bf angle_ef_error_rad _angle_bf_error_rad frame_conversion_ef_to_bf _rate_ef_desired_rads _rate_bf_desired_rads _rate_bf_target_rads += _rate_bf_desired_rads void AC_AttitudeControl::rate_bf_roll_pitch_yaw float roll_rate_bf_cds float pitch_rate_bf_cds float yaw_rate_bf_cds float roll_rate_bf_rads = radians roll_rate_bf_cds*0.01f float pitch_rate_bf_rads = radians pitch_rate_bf_cds*0.01f float yaw_rate_bf_rads = radians yaw_rate_bf_cds*0.01f Vector3f angle_ef_error_rad float rate_change_rads rate_change_limit_rads if get_accel_roll_max_radss > 0.0f rate_change_limit_rads = get_accel_roll_max_radss * _dt rate_change_rads = roll_rate_bf_rads - _rate_bf_desired_rads.x rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_bf_desired_rads.x += rate_change_rads _rate_bf_desired_rads.x = roll_rate_bf_rads if get_accel_pitch_max_radss > 0.0f rate_change_limit_rads = get_accel_pitch_max_radss * _dt rate_change_rads = pitch_rate_bf_rads - _rate_bf_desired_rads.y rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_bf_desired_rads.y += rate_change_rads _rate_bf_desired_rads.y = pitch_rate_bf_rads if get_accel_yaw_max_radss > 0.0f rate_change_limit_rads = get_accel_yaw_max_radss * _dt rate_change_rads = yaw_rate_bf_rads - _rate_bf_desired_rads.z rate_change_rads = constrain_float rate_change_rads -rate_change_limit_rads rate_change_limit_rads _rate_bf_desired_rads.z += rate_change_rads _rate_bf_desired_rads.z = yaw_rate_bf_rads if fabsf _ahrs.pitch <_acro_angle_switch_rad _acro_angle_switch_rad = radians 60.0f frame_conversion_bf_to_ef _rate_bf_desired_rads _rate_ef_desired_rads update_ef_roll_angle_and_error _rate_ef_desired_rads.x angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD update_ef_pitch_angle_and_error _rate_ef_desired_rads.y angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD update_ef_yaw_angle_and_error _rate_ef_desired_rads.z angle_ef_error_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD frame_conversion_ef_to_bf angle_ef_error_rad _angle_bf_error_rad _acro_angle_switch_rad = radians 45.0f if frame_conversion_bf_to_ef _angle_bf_error_rad angle_ef_error_rad _angle_ef_target_rad.x = wrap_PI angle_ef_error_rad.x + _ahrs.roll _angle_ef_target_rad.y = wrap_PI angle_ef_error_rad.y + _ahrs.pitch _angle_ef_target_rad.z = wrap_2PI angle_ef_error_rad.z + _ahrs.yaw if _angle_ef_target_rad.y > M_PI/2.0f _angle_ef_target_rad.x = wrap_PI _angle_ef_target_rad.x + M_PI _angle_ef_target_rad.y = wrap_PI M_PI - _angle_ef_target_rad.y _angle_ef_target_rad.z = wrap_2PI _angle_ef_target_rad.z + M_PI if _angle_ef_target_rad.y < -M_PI/2.0f _angle_ef_target_rad.x = wrap_PI _angle_ef_target_rad.x + M_PI _angle_ef_target_rad.y = wrap_PI -M_PI - _angle_ef_target_rad.y _angle_ef_target_rad.z = wrap_2PI _angle_ef_target_rad.z + M_PI _rate_bf_target_rads += _rate_bf_desired_rads _motors.set_roll rate_bf_to_motor_roll _rate_bf_target_rads.x _motors.set_pitch rate_bf_to_motor_pitch _rate_bf_target_rads.y _motors.set_yaw rate_bf_to_motor_yaw _rate_bf_target_rads.z void AC_AttitudeControl::update_ef_roll_angle_and_error float roll_rate_ef_rads Vector3f &angle_ef_error_rad float overshoot_max_rad angle_ef_error_rad.x = wrap_PI _angle_ef_target_rad.x - _ahrs.roll angle_ef_error_rad.x = constrain_float angle_ef_error_rad.x -overshoot_max_rad overshoot_max_rad _angle_ef_target_rad.x = angle_ef_error_rad.x + _ahrs.roll _angle_ef_target_rad.x += roll_rate_ef_rads * _dt _angle_ef_target_rad.x = wrap_PI _angle_ef_target_rad.x void AC_AttitudeControl::update_ef_pitch_angle_and_error float pitch_rate_ef_rads Vector3f &angle_ef_error_rad float overshoot_max_rad angle_ef_error_rad.y = wrap_PI _angle_ef_target_rad.y - _ahrs.pitch angle_ef_error_rad.y = constrain_float angle_ef_error_rad.y -overshoot_max_rad overshoot_max_rad _angle_ef_target_rad.y = angle_ef_error_rad.y + _ahrs.pitch _angle_ef_target_rad.y += pitch_rate_ef_rads * _dt _angle_ef_target_rad.y = wrap_PI _angle_ef_target_rad.y void AC_AttitudeControl::update_ef_yaw_angle_and_error float yaw_rate_ef_rads Vector3f &angle_ef_error_rad float overshoot_max_rad angle_ef_error_rad.z = wrap_PI _angle_ef_target_rad.z - _ahrs.yaw angle_ef_error_rad.z = constrain_float angle_ef_error_rad.z -overshoot_max_rad overshoot_max_rad _angle_ef_target_rad.z = angle_ef_error_rad.z + _ahrs.yaw _angle_ef_target_rad.z += yaw_rate_ef_rads * _dt _angle_ef_target_rad.z = wrap_2PI _angle_ef_target_rad.z _angle_bf_error_rad.x += _rate_bf_desired_rads.x - _ahrs.get_gyro .x * _dt _angle_bf_error_rad.x = constrain_float _angle_bf_error_rad.x -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD _angle_bf_error_rad.y += _rate_bf_desired_rads.y - _ahrs.get_gyro .y * _dt _angle_bf_error_rad.y = constrain_float _angle_bf_error_rad.y -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD _angle_bf_error_rad.z += _rate_bf_desired_rads.z - _ahrs.get_gyro .z * _dt _angle_bf_error_rad.z = constrain_float _angle_bf_error_rad.z -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD _rate_bf_target_rads.x = sqrt_controller _angle_bf_error_rad.x _p_angle_roll.kP constrain_float get_accel_roll_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS _rate_bf_target_rads.x = _p_angle_roll.kP * _angle_bf_error_rad.x _rate_bf_target_rads.y = sqrt_controller _angle_bf_error_rad.y _p_angle_pitch.kP constrain_float get_accel_pitch_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS _rate_bf_target_rads.y = _p_angle_pitch.kP * _angle_bf_error_rad.y _rate_bf_target_rads.z = sqrt_controller _angle_bf_error_rad.z _p_angle_yaw.kP constrain_float get_accel_yaw_max_radss /2.0f AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS _rate_bf_target_rads.z = _p_angle_yaw.kP * _angle_bf_error_rad.z _rate_bf_target_rads.x += _angle_bf_error_rad.y * _ahrs.get_gyro .z _rate_bf_target_rads.y += -_angle_bf_error_rad.x * _ahrs.get_gyro .z float AC_AttitudeControl::rate_bf_to_motor_roll float rate_target_rads float current_rate_rads float rate_error_rads current_rate_rads = _ahrs.get_gyro .x rate_error_rads = rate_target_rads - current_rate_rads _pid_rate_roll.set_input_filter_d degrees rate_error_rads *100.0f _pid_rate_roll.set_desired_rate degrees rate_target_rads *100.0f if !_motors.limit.roll_pitch || i>0&&rate_error_rads<0 || i<0&&rate_error_rads>0 float AC_AttitudeControl::rate_bf_to_motor_pitch float rate_target_rads float current_rate_rads float rate_error_rads current_rate_rads = _ahrs.get_gyro .y rate_error_rads = rate_target_rads - current_rate_rads _pid_rate_pitch.set_input_filter_d degrees rate_error_rads *100.0f _pid_rate_pitch.set_desired_rate degrees rate_target_rads *100.0f if !_motors.limit.roll_pitch || i>0&&rate_error_rads<0 || i<0&&rate_error_rads>0 float AC_AttitudeControl::rate_bf_to_motor_yaw float rate_target_rads float current_rate_rads float rate_error_rads current_rate_rads = _ahrs.get_gyro .z rate_error_rads = rate_target_rads - current_rate_rads _pid_rate_yaw.set_input_filter_all degrees rate_error_rads *100.0f _pid_rate_yaw.set_desired_rate degrees rate_target_rads *100.0f if !_motors.limit.yaw || i>0&&rate_error_rads<0 || i<0&&rate_error_rads>0 "
ArduPilot 257 AC_AttitudeControl.cpp Control 06c8457efd AC_AttitudeControl: rename and modify frame_conversion functions to follow conventions " ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _rate_bf_target_rads _rate_ef_desired_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw angle_ef_error_rad _angle_bf_error_rad euler_derivative_to_ang_vel _angle_ef_target_rad _rate_ef_desired_rads _rate_bf_desired_rads euler_derivative_to_ang_vel _angle_ef_target_rad Vector3f 0 0 _rate_ef_desired_rads.z _rate_bf_desired_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw angle_ef_error_rad _angle_bf_error_rad euler_derivative_to_ang_vel _angle_ef_target_rad _rate_ef_desired_rads _rate_bf_desired_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw angle_ef_error_rad _angle_bf_error_rad euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw angle_ef_error_rad _angle_bf_error_rad euler_derivative_to_ang_vel _angle_ef_target_rad _rate_ef_desired_rads _rate_bf_desired_rads ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _rate_bf_desired_rads _rate_ef_desired_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw angle_ef_error_rad _angle_bf_error_rad if ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _angle_bf_error_rad angle_ef_error_rad void AC_AttitudeControl::euler_derivative_to_ang_vel const Vector3f& euler_rad const Vector3f& euler_dot_rads Vector3f& ang_vel_rads float sin_theta = sinf euler_rad.y float cos_theta = cosf euler_rad.y float sin_phi = sinf euler_rad.x float cos_phi = cosf euler_rad.x ang_vel_rads.x = euler_dot_rads.x - sin_theta * euler_dot_rads.z ang_vel_rads.y = cos_phi * euler_dot_rads.y + sin_phi * cos_theta * euler_dot_rads.z ang_vel_rads.z = -sin_phi * euler_dot_rads.y + cos_theta * cos_phi * euler_dot_rads.z bool AC_AttitudeControl::ang_vel_to_euler_derivative const Vector3f& euler_rad const Vector3f& ang_vel_rads Vector3f& euler_dot_rads float sin_theta = sinf euler_rad.y float cos_theta = cosf euler_rad.y float sin_phi = sinf euler_rad.x float cos_phi = cosf euler_rad.x if is_zero cos_theta euler_dot_rads.x = ang_vel_rads.x + sin_phi * sin_theta/cos_theta * ang_vel_rads.y + cos_phi * sin_theta/cos_theta * ang_vel_rads.z euler_dot_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z euler_dot_rads.z = sin_phi / cos_theta * ang_vel_rads.y + cos_phi / cos_theta * ang_vel_rads.z "
ArduPilot 258 AC_AttitudeControl.cpp Control 1afab89991 AC_AttitudeControl: extensive renaming and recommenting " _ang_vel_target_rads = _ahrs.get_gyro ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_deriv_rads _att_target_euler_rad.z = wrap_2PI _att_target_euler_rad.z + radians yaw_shift_cd*0.01f void AC_AttitudeControl::euler_angle_roll_pitch_euler_rate_yaw_smooth float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_rate_cds float smoothing_gain float euler_roll_angle_rad = radians euler_roll_angle_cd*0.01f float euler_pitch_angle_rad = radians euler_pitch_angle_cd*0.01f float euler_yaw_rate_rads = radians euler_yaw_rate_cds*0.01f euler_roll_angle_rad += get_roll_trim_rad Vector3f att_error_euler_rad if get_accel_roll_max_radss > 0.0f && _rate_bf_ff_enabled float rate_change_limit_rads = get_accel_roll_max_radss * _dt float euler_rate_desired_rads = sqrt_controller euler_roll_angle_rad-_att_target_euler_rad.x smoothing_gain get_accel_roll_max_radss _att_target_euler_deriv_rads.x = constrain_float euler_rate_desired_rads _att_target_euler_deriv_rads.x-rate_change_limit_rads _att_target_euler_deriv_rads.x+rate_change_limit_rads update_att_target_and_error_roll _att_target_euler_deriv_rads.x att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rad.x = euler_roll_angle_rad att_error_euler_rad.x = wrap_180_cd_float _att_target_euler_rad.x - _ahrs.roll _att_target_euler_deriv_rads.x = 0 _att_target_euler_rad.x = constrain_float _att_target_euler_rad.x -get_tilt_limit_rad get_tilt_limit_rad float rate_change_limit_rads = get_accel_pitch_max_radss * _dt float euler_rate_desired_rads = sqrt_controller euler_pitch_angle_rad-_att_target_euler_rad.y smoothing_gain get_accel_pitch_max_radss _att_target_euler_deriv_rads.y = constrain_float euler_rate_desired_rads _att_target_euler_deriv_rads.y-rate_change_limit_rads _att_target_euler_deriv_rads.y+rate_change_limit_rads update_att_target_and_error_pitch _att_target_euler_deriv_rads.y att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rad.y = euler_pitch_angle_rad att_error_euler_rad.y = wrap_180_cd_float _att_target_euler_rad.y - _ahrs.pitch _att_target_euler_deriv_rads.y = 0 _att_target_euler_rad.y = constrain_float _att_target_euler_rad.y -get_tilt_limit_rad get_tilt_limit_rad float rate_change_limit_rads = get_accel_yaw_max_radss * _dt _att_target_euler_deriv_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_deriv_rads.z -rate_change_limit_rads rate_change_limit_rads update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_deriv_rads.z = euler_yaw_rate_rads update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad update_ang_vel_target_from_att_error euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_deriv_rads _att_target_ang_vel_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw Vector3f 0 0 _att_target_euler_deriv_rads.z _att_target_ang_vel_rads _ang_vel_target_rads += _att_target_ang_vel_rads void AC_AttitudeControl::euler_angle_roll_pitch_euler_rate_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_rate_cds float euler_roll_angle_rad = radians euler_roll_angle_cd*0.01f float euler_pitch_angle_rad = radians euler_pitch_angle_cd*0.01f float euler_yaw_rate_rads = radians euler_yaw_rate_cds*0.01f Vector3f att_error_euler_rad euler_roll_angle_rad += get_roll_trim_rad _att_target_euler_rad.x = constrain_float euler_roll_angle_rad -get_tilt_limit_rad get_tilt_limit_rad _att_target_euler_rad.y = constrain_float euler_pitch_angle_rad -get_tilt_limit_rad get_tilt_limit_rad att_error_euler_rad.x = wrap_PI _att_target_euler_rad.x - _ahrs.roll att_error_euler_rad.y = wrap_PI _att_target_euler_rad.y - _ahrs.pitch _att_target_euler_deriv_rads.x = 0 _att_target_euler_deriv_rads.y = 0 _att_target_euler_deriv_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_deriv_rads.z -rate_change_limit_rads rate_change_limit_rads update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_deriv_rads.z = euler_yaw_rate_rads update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad update_ang_vel_target_from_att_error euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_deriv_rads _att_target_ang_vel_rads _ang_vel_target_rads += _att_target_ang_vel_rads void AC_AttitudeControl::euler_angle_roll_pitch_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_angle_cd bool slew_yaw float euler_roll_angle_rad = radians euler_roll_angle_cd*0.01f float euler_pitch_angle_rad = radians euler_pitch_angle_cd*0.01f float yaw_angle_ef_rad = radians euler_yaw_angle_cd*0.01f Vector3f att_error_euler_rad euler_roll_angle_rad += get_roll_trim_rad _att_target_euler_rad.x = constrain_float euler_roll_angle_rad -get_tilt_limit_rad get_tilt_limit_rad _att_target_euler_rad.y = constrain_float euler_pitch_angle_rad -get_tilt_limit_rad get_tilt_limit_rad _att_target_euler_rad.z = yaw_angle_ef_rad att_error_euler_rad.x = wrap_PI _att_target_euler_rad.x - _ahrs.roll att_error_euler_rad.y = wrap_PI _att_target_euler_rad.y - _ahrs.pitch att_error_euler_rad.z = wrap_PI _att_target_euler_rad.z - _ahrs.yaw att_error_euler_rad.z = constrain_float att_error_euler_rad.z -get_slew_yaw_rads get_slew_yaw_rads euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad update_ang_vel_target_from_att_error void AC_AttitudeControl::euler_rate_roll_pitch_yaw float euler_roll_rate_cds float euler_pitch_rate_cds float euler_yaw_rate_cds float euler_roll_rate_rads = radians euler_roll_rate_cds*0.01f float euler_pitch_rate_rads = radians euler_pitch_rate_cds*0.01f float euler_yaw_rate_rads = radians euler_yaw_rate_cds*0.01f Vector3f att_error_euler_rad float rate_change_limit_rads = get_accel_roll_max_radss * _dt _att_target_euler_deriv_rads.x += constrain_float euler_roll_rate_rads - _att_target_euler_deriv_rads.x -rate_change_limit_rads rate_change_limit_rads _att_target_euler_deriv_rads.x = euler_roll_rate_rads float rate_change_limit_rads = get_accel_pitch_max_radss * _dt _att_target_euler_deriv_rads.y += constrain_float euler_pitch_rate_rads - _att_target_euler_deriv_rads.y -rate_change_limit_rads rate_change_limit_rads _att_target_euler_deriv_rads.y = euler_pitch_rate_rads float rate_change_limit_rads = get_accel_yaw_max_radss * _dt _att_target_euler_deriv_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_deriv_rads.z -rate_change_limit_rads rate_change_limit_rads _att_target_euler_deriv_rads.z = euler_yaw_rate_rads update_att_target_and_error_roll _att_target_euler_deriv_rads.x att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_pitch _att_target_euler_deriv_rads.y att_error_euler_rad AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rad.x = constrain_float _att_target_euler_rad.x -get_tilt_limit_rad get_tilt_limit_rad _att_target_euler_rad.y = constrain_float _att_target_euler_rad.y -get_tilt_limit_rad get_tilt_limit_rad euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad update_ang_vel_target_from_att_error euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_deriv_rads _att_target_ang_vel_rads _ang_vel_target_rads += _att_target_ang_vel_rads Vector3f att_error_euler_rad float rate_change_limit_rads = get_accel_roll_max_radss * _dt _att_target_ang_vel_rads.x += constrain_float roll_rate_bf_rads - _att_target_ang_vel_rads.x -rate_change_limit_rads rate_change_limit_rads _att_target_ang_vel_rads.x = roll_rate_bf_rads float rate_change_limit_rads = get_accel_pitch_max_radss * _dt _att_target_ang_vel_rads.y += constrain_float pitch_rate_bf_rads - _att_target_ang_vel_rads.y -rate_change_limit_rads rate_change_limit_rads _att_target_ang_vel_rads.y = pitch_rate_bf_rads float rate_change_limit_rads = get_accel_yaw_max_radss * _dt _att_target_ang_vel_rads.z += constrain_float yaw_rate_bf_rads - _att_target_ang_vel_rads.z -rate_change_limit_rads rate_change_limit_rads _att_target_ang_vel_rads.z = yaw_rate_bf_rads ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_ang_vel_rads _att_target_euler_deriv_rads update_att_target_and_error_roll _att_target_euler_deriv_rads.x att_error_euler_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_pitch _att_target_euler_deriv_rads.y att_error_euler_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_yaw _att_target_euler_deriv_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD euler_derivative_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad if ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_error_rot_vec_rad att_error_euler_rad _att_target_euler_rad.x = wrap_PI att_error_euler_rad.x + _ahrs.roll _att_target_euler_rad.y = wrap_PI att_error_euler_rad.y + _ahrs.pitch _att_target_euler_rad.z = wrap_2PI att_error_euler_rad.z + _ahrs.yaw if _att_target_euler_rad.y > M_PI/2.0f _att_target_euler_rad.x = wrap_PI _att_target_euler_rad.x + M_PI _att_target_euler_rad.y = wrap_PI M_PI - _att_target_euler_rad.y _att_target_euler_rad.z = wrap_2PI _att_target_euler_rad.z + M_PI if _att_target_euler_rad.y < -M_PI/2.0f _att_target_euler_rad.x = wrap_PI _att_target_euler_rad.x + M_PI _att_target_euler_rad.y = wrap_PI -M_PI - _att_target_euler_rad.y _att_target_euler_rad.z = wrap_2PI _att_target_euler_rad.z + M_PI update_ang_vel_target_from_att_error _ang_vel_target_rads += _att_target_ang_vel_rads _motors.set_roll rate_bf_to_motor_roll _ang_vel_target_rads.x _motors.set_pitch rate_bf_to_motor_pitch _ang_vel_target_rads.y _motors.set_yaw rate_bf_to_motor_yaw _ang_vel_target_rads.z void AC_AttitudeControl::update_att_target_and_error_roll float euler_roll_rate_rads Vector3f &att_error_euler_rad float overshoot_max_rad att_error_euler_rad.x = wrap_PI _att_target_euler_rad.x - _ahrs.roll att_error_euler_rad.x = constrain_float att_error_euler_rad.x -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.x = att_error_euler_rad.x + _ahrs.roll _att_target_euler_rad.x += euler_roll_rate_rads * _dt _att_target_euler_rad.x = wrap_PI _att_target_euler_rad.x void AC_AttitudeControl::update_att_target_and_error_pitch float euler_pitch_rate_rads Vector3f &att_error_euler_rad float overshoot_max_rad att_error_euler_rad.y = wrap_PI _att_target_euler_rad.y - _ahrs.pitch att_error_euler_rad.y = constrain_float att_error_euler_rad.y -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.y = att_error_euler_rad.y + _ahrs.pitch _att_target_euler_rad.y += euler_pitch_rate_rads * _dt _att_target_euler_rad.y = wrap_PI _att_target_euler_rad.y void AC_AttitudeControl::update_att_target_and_error_yaw float euler_yaw_rate_rads Vector3f &att_error_euler_rad float overshoot_max_rad att_error_euler_rad.z = wrap_PI _att_target_euler_rad.z - _ahrs.yaw att_error_euler_rad.z = constrain_float att_error_euler_rad.z -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.z = att_error_euler_rad.z + _ahrs.yaw _att_target_euler_rad.z += euler_yaw_rate_rads * _dt _att_target_euler_rad.z = wrap_2PI _att_target_euler_rad.z _att_error_rot_vec_rad += _att_target_ang_vel_rads - _ahrs.get_gyro * _dt _att_error_rot_vec_rad.x = constrain_float _att_error_rot_vec_rad.x -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD _att_error_rot_vec_rad.y = constrain_float _att_error_rot_vec_rad.y -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD _att_error_rot_vec_rad.z = constrain_float _att_error_rot_vec_rad.z -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD void AC_AttitudeControl::update_ang_vel_target_from_att_error if _att_ctrl_use_accel_limit && _accel_roll_max > 0.0f _ang_vel_target_rads.x = sqrt_controller _att_error_rot_vec_rad.x _p_angle_roll.kP constrain_float get_accel_roll_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS _ang_vel_target_rads.x = _p_angle_roll.kP * _att_error_rot_vec_rad.x if _att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f _ang_vel_target_rads.y = sqrt_controller _att_error_rot_vec_rad.y _p_angle_pitch.kP constrain_float get_accel_pitch_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS _ang_vel_target_rads.y = _p_angle_pitch.kP * _att_error_rot_vec_rad.y if _att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f _ang_vel_target_rads.z = sqrt_controller _att_error_rot_vec_rad.z _p_angle_yaw.kP constrain_float get_accel_yaw_max_radss /2.0f AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS _ang_vel_target_rads.z = _p_angle_yaw.kP * _att_error_rot_vec_rad.z _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro .z _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro .z float current_rate_rads = _ahrs.get_gyro .x float rate_error_rads = rate_target_rads - current_rate_rads float integrator = _pid_rate_roll.get_integrator if !_motors.limit.roll_pitch || integrator > 0 && rate_error_rads < 0 || integrator < 0 && rate_error_rads > 0 integrator = _pid_rate_roll.get_i float output = _pid_rate_roll.get_p + integrator + _pid_rate_roll.get_d return constrain_float output -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX float current_rate_rads = _ahrs.get_gyro .y float rate_error_rads = rate_target_rads - current_rate_rads float integrator = _pid_rate_pitch.get_integrator if !_motors.limit.roll_pitch || integrator > 0 && rate_error_rads < 0 || integrator < 0 && rate_error_rads > 0 integrator = _pid_rate_pitch.get_i float output = _pid_rate_pitch.get_p + integrator + _pid_rate_pitch.get_d return constrain_float output -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX float current_rate_rads = _ahrs.get_gyro .z float rate_error_rads = rate_target_rads - current_rate_rads _pid_rate_yaw.set_input_filter_d degrees rate_error_rads *100.0f float integrator = _pid_rate_yaw.get_integrator if !_motors.limit.yaw || integrator > 0 && rate_error_rads < 0 || integrator < 0 && rate_error_rads > 0 integrator = _pid_rate_yaw.get_i float output = _pid_rate_yaw.get_p + integrator + _pid_rate_yaw.get_d return constrain_float output -AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX "
ArduPilot 259 AC_AttitudeControl.cpp Control edda7e4e1e AC_AttitudeControl: keep _att_target_euler_deriv_rads updated in euler_angle_roll_pitch_yaw " ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_deriv_rads "
ArduPilot 260 AC_AttitudeControl.cpp Non-Control 8b886bc479 AC_AttitudeControl: minor comment changes and reorganization " float rate_change_limit_rads = get_accel_roll_max_radss * _dt float rate_change_limit_rads = get_accel_pitch_max_radss * _dt "
ArduPilot 261 AC_AttitudeControl.cpp Control f8c709478a AC_AttitudeControl: quaternion acro controller " Quaternion att_target_quat att_target_quat.from_euler _att_target_euler_rad.x _att_target_euler_rad.y _att_target_euler_rad.z att_target_quat.rotate _att_target_ang_vel_rads*_dt att_target_quat.normalize att_target_quat.to_euler _att_target_euler_rad.x _att_target_euler_rad.y _att_target_euler_rad.z Quaternion att_vehicle_quat att_vehicle_quat.from_rotation_matrix _ahrs.get_dcm_matrix att_vehicle_quat.inverse *att_target_quat .to_axis_angle _att_error_rot_vec_rad ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_ang_vel_rads _att_target_euler_deriv_rads "
ArduPilot 262 AC_AttitudeControl.cpp Non-Control bba360ea2b AC_AttitudeControl: correct bugs found in review " att_error_euler_rad.x = wrap_PI _att_target_euler_rad.x - _ahrs.roll att_error_euler_rad.y = wrap_PI _att_target_euler_rad.y - _ahrs.pitch "
ArduPilot 263 AC_AttitudeControl.cpp Non-Control 5919e95635 AC_AttitudeControl: add input_ prefix to input shaper functions " void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_rate_cds float smoothing_gain void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_rate_cds void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_angle_cd bool slew_yaw void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw float euler_roll_rate_cds float euler_pitch_rate_cds float euler_yaw_rate_cds void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw float roll_rate_bf_cds float pitch_rate_bf_cds float yaw_rate_bf_cds "
ArduPilot 264 AC_AttitudeControl.cpp Non-Control 41e580e53a AC_AttitudeControl: add input_att_quat_bf_ang_vel and use for ACRO " input_att_quat_bf_ang_vel att_target_quat _att_target_ang_vel_rads void AC_AttitudeControl::input_att_quat_bf_ang_vel const Quaternion& att_target_quat const Vector3f& ang_vel_target_rads _att_target_ang_vel_rads = ang_vel_target_rads ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw ang_vel_target_rads _att_target_euler_deriv_rads "
ArduPilot 265 AC_AttitudeControl.cpp Non-Control 0baf86c485 AC_AttitudeControl: fixup more names " float euler_yaw_angle_rad = radians euler_yaw_angle_cd*0.01f _att_target_euler_rad.z = euler_yaw_angle_rad void AC_AttitudeControl::input_att_quat_bf_ang_vel const Quaternion& att_target_quat const Vector3f& att_target_ang_vel_rads _att_target_ang_vel_rads = att_target_ang_vel_rads ang_vel_to_euler_derivative Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_target_ang_vel_rads _att_target_euler_deriv_rads "
ArduPilot 266 AC_AttitudeControl.cpp Non-Control 162d2a9112 AC_AttitudeControl: naming changes in response to Leonard s review " ang_vel_to_euler_rate Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_rate_rads _att_target_euler_rate_rads.x = constrain_float euler_rate_desired_rads _att_target_euler_rate_rads.x-rate_change_limit_rads _att_target_euler_rate_rads.x+rate_change_limit_rads update_att_target_and_error_roll _att_target_euler_rate_rads.x att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rate_rads.x = 0 _att_target_euler_rate_rads.y = constrain_float euler_rate_desired_rads _att_target_euler_rate_rads.y-rate_change_limit_rads _att_target_euler_rate_rads.y+rate_change_limit_rads update_att_target_and_error_pitch _att_target_euler_rate_rads.y att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rate_rads.y = 0 _att_target_euler_rate_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_rate_rads.z -rate_change_limit_rads rate_change_limit_rads update_att_target_and_error_yaw _att_target_euler_rate_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rate_rads.z = euler_yaw_rate_rads update_att_target_and_error_yaw _att_target_euler_rate_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_rate_rads _att_target_ang_vel_rads euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw Vector3f 0 0 _att_target_euler_rate_rads.z _att_target_ang_vel_rads _att_target_euler_rate_rads.x = 0 _att_target_euler_rate_rads.y = 0 _att_target_euler_rate_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_rate_rads.z -rate_change_limit_rads rate_change_limit_rads update_att_target_and_error_yaw _att_target_euler_rate_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD _att_target_euler_rate_rads.z = euler_yaw_rate_rads update_att_target_and_error_yaw _att_target_euler_rate_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_rate_rads _att_target_ang_vel_rads euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad ang_vel_to_euler_rate Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_rate_rads _att_target_euler_rate_rads.x += constrain_float euler_roll_rate_rads - _att_target_euler_rate_rads.x -rate_change_limit_rads rate_change_limit_rads _att_target_euler_rate_rads.x = euler_roll_rate_rads _att_target_euler_rate_rads.y += constrain_float euler_pitch_rate_rads - _att_target_euler_rate_rads.y -rate_change_limit_rads rate_change_limit_rads _att_target_euler_rate_rads.y = euler_pitch_rate_rads _att_target_euler_rate_rads.z += constrain_float euler_yaw_rate_rads - _att_target_euler_rate_rads.z -rate_change_limit_rads rate_change_limit_rads _att_target_euler_rate_rads.z = euler_yaw_rate_rads update_att_target_and_error_roll _att_target_euler_rate_rads.x att_error_euler_rad AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_pitch _att_target_euler_rate_rads.y att_error_euler_rad AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD update_att_target_and_error_yaw _att_target_euler_rate_rads.z att_error_euler_rad AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_error_euler_rad _att_error_rot_vec_rad euler_rate_to_ang_vel Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _att_target_euler_rate_rads _att_target_ang_vel_rads ang_vel_to_euler_rate Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw att_target_ang_vel_rads _att_target_euler_rate_rads void AC_AttitudeControl::euler_rate_to_ang_vel const Vector3f& euler_rad const Vector3f& euler_dot_rads Vector3f& ang_vel_rads bool AC_AttitudeControl::ang_vel_to_euler_rate const Vector3f& euler_rad const Vector3f& ang_vel_rads Vector3f& euler_dot_rads "
ArduPilot 267 AC_AttitudeControl.cpp Control b8223771d3 AC_AttitudeControl: properly protect sqrt_controller from nonpositive acceleration limits " if second_ord_lim < 0.0f || is_zero second_ord_lim || is_zero p "
ArduPilot 268 AC_AttitudeControl.cpp Non-Control 846ee7d2af AC_AttitudeControl: fix regression in angular velocity controller " _pid_rate_yaw.set_input_filter_all degrees rate_error_rads *100.0f "
ArduPilot 269 AC_AttitudeControl.cpp Non-Control 3a06edcee3 AC_AttitudeControl: rename local variable to match naming convention " void AC_AttitudeControl::euler_rate_to_ang_vel const Vector3f& euler_rad const Vector3f& euler_rate_rads Vector3f& ang_vel_rads ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z bool AC_AttitudeControl::ang_vel_to_euler_rate const Vector3f& euler_rad const Vector3f& ang_vel_rads Vector3f& euler_rate_rads euler_rate_rads.x = ang_vel_rads.x + sin_phi * sin_theta/cos_theta * ang_vel_rads.y + cos_phi * sin_theta/cos_theta * ang_vel_rads.z euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z euler_rate_rads.z = sin_phi / cos_theta * ang_vel_rads.y + cos_phi / cos_theta * ang_vel_rads.z "
ArduPilot 270 AC_AttitudeControl.cpp Non-Control 02eda4dcab AC_AttControl: remove comment fix formatting ""
ArduPilot 271 AC_AttitudeControl.cpp Non-Control ee8090e25f AC_AttitudeControl: reflect renamed function in AP_AHRS " att_vehicle_quat.from_rotation_matrix _ahrs.get_rotation_body_to_ned "
ArduPilot 272 AC_AttitudeControl.cpp Non-Control 3d4bd92b48 AC_AttitudeControl: add helper functions to retrieve rotation matrices " void AC_AttitudeControl::get_rotation_vehicle_to_ned Matrix3f& m m = _ahrs.get_dcm_matrix void AC_AttitudeControl::get_rotation_ned_to_vehicle Matrix3f& m get_rotation_vehicle_to_ned m m = m.transposed void AC_AttitudeControl::get_rotation_reference_to_ned Matrix3f& m m.from_euler _att_target_euler_rad.x _att_target_euler_rad.y _att_target_euler_rad.z void AC_AttitudeControl::get_rotation_ned_to_reference Matrix3f& m get_rotation_reference_to_ned m m = m.transposed void AC_AttitudeControl::get_rotation_vehicle_to_reference Matrix3f& m Matrix3f Tvn Matrix3f Tnr get_rotation_vehicle_to_ned Tvn get_rotation_ned_to_reference Tnr m = Tnr * Tvn void AC_AttitudeControl::get_rotation_reference_to_vehicle Matrix3f& m get_rotation_vehicle_to_reference m m = m.transposed "
ArduPilot 273 AC_AttitudeControl.cpp Control ae3357baee AC_AttitudeControl: rotate angular velocity feedforward into vehicle frame " Matrix3f Trv get_rotation_reference_to_vehicle Trv _ang_vel_target_rads += Trv * _att_target_ang_vel_rads Matrix3f Trv get_rotation_reference_to_vehicle Trv _ang_vel_target_rads += Trv * _att_target_ang_vel_rads Matrix3f Trv get_rotation_reference_to_vehicle Trv _ang_vel_target_rads += Trv * _att_target_ang_vel_rads Matrix3f Trv get_rotation_reference_to_vehicle Trv _ang_vel_target_rads += Trv * _att_target_ang_vel_rads "
ArduPilot 274 AC_AttitudeControl.cpp Control 829d6037fc AC_AttitudeControl: convert euler rates to angular velocity using the correct attitude " euler_rate_to_ang_vel _att_target_euler_rad _att_target_euler_rate_rads _att_target_ang_vel_rads euler_rate_to_ang_vel _att_target_euler_rad Vector3f 0 0 _att_target_euler_rate_rads.z _att_target_ang_vel_rads euler_rate_to_ang_vel _att_target_euler_rad _att_target_euler_rate_rads _att_target_ang_vel_rads euler_rate_to_ang_vel _att_target_euler_rad _att_target_euler_rate_rads _att_target_ang_vel_rads ang_vel_to_euler_rate _att_target_euler_rad att_target_ang_vel_rads _att_target_euler_rate_rads "
ArduPilot 275 AC_AttitudeControl.cpp Non-Control 46e6ff3ca9 AC_AttitudeControl: modify comment on kinematic correction feedforward ""
ArduPilot 276 AC_AttitudeControl.cpp Non-Control 86bb03aa79 AC_AttControl: use ahrs get_rotation_body_to_nedahrs.get_dcm_matrix renamed to ahrs.get_rotation_body_to_ned " m = _ahrs.get_rotation_body_to_ned "
ArduPilot 277 AC_AttitudeControl.cpp Control 5ddd332277 AC_AttControl: add ANGLE_BOOST paramThis allows disabling angle boost " AP_GROUPINFO ANGLE_BOOST 12 AC_AttitudeControl _angle_boost_enabled 1 "
ArduPilot 278 AC_AttitudeControl.cpp Control e946e047e6 AC_AttitudeControl: add attitude_controller_run functions call from input functions " update_att_target_roll _att_target_euler_rate_rads.x AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_att_target_pitch _att_target_euler_rate_rads.y AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_att_target_yaw _att_target_euler_rate_rads.z AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD update_att_target_yaw _att_target_euler_rate_rads.z AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD attitude_controller_run_euler _att_target_euler_rad _att_target_ang_vel_rads update_att_target_yaw _att_target_euler_rate_rads.z AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD update_att_target_yaw _att_target_euler_rate_rads.z AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD attitude_controller_run_euler _att_target_euler_rad _att_target_ang_vel_rads float angle_error = constrain_float wrap_PI _att_target_euler_rad.z - _ahrs.yaw -get_slew_yaw_rads get_slew_yaw_rads _att_target_euler_rad.z = angle_error + _ahrs.yaw attitude_controller_run_euler _att_target_euler_rad Vector3f 0.0f 0.0f 0.0f update_att_target_roll _att_target_euler_rate_rads.x AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD update_att_target_pitch _att_target_euler_rate_rads.y AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD update_att_target_yaw _att_target_euler_rate_rads.z AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD attitude_controller_run_euler _att_target_euler_rad _att_target_ang_vel_rads attitude_controller_run_quat att_target_quat _att_target_ang_vel_rads ang_vel_to_euler_rate Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_rate_rads attitude_controller_run_quat att_target_quat att_target_ang_vel_rads ang_vel_to_euler_rate Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _ang_vel_target_rads _att_target_euler_rate_rads void AC_AttitudeControl::attitude_controller_run_euler const Vector3f& att_target_euler_rad const Vector3f& att_target_ang_vel_rads Quaternion att_target_quat att_target_quat.from_euler att_target_euler_rad.x att_target_euler_rad.y att_target_euler_rad.z attitude_controller_run_quat att_target_quat att_target_ang_vel_rads void AC_AttitudeControl::attitude_controller_run_quat const Quaternion& att_target_quat const Vector3f& att_target_ang_vel_rads void AC_AttitudeControl::update_att_target_roll float euler_roll_rate_rads float overshoot_max_rad float angle_error = constrain_float wrap_PI _att_target_euler_rad.x - _ahrs.roll -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.x = angle_error + _ahrs.roll void AC_AttitudeControl::update_att_target_pitch float euler_pitch_rate_rads float overshoot_max_rad float angle_error = constrain_float wrap_PI _att_target_euler_rad.y - _ahrs.pitch -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.y = angle_error + _ahrs.pitch void AC_AttitudeControl::update_att_target_yaw float euler_yaw_rate_rads float overshoot_max_rad float angle_error = constrain_float wrap_PI _att_target_euler_rad.z - _ahrs.yaw -overshoot_max_rad overshoot_max_rad _att_target_euler_rad.z = angle_error + _ahrs.yaw "
ArduPilot 279 AC_AttitudeControl.cpp Control c0f209fa42 AC_AttControl: angle_boost to float " _angle_boost = 0.0f _angle_boost = 0.0f "
ArduPilot 280 AC_AttitudeControl.cpp Non-Control 2822b93cd4 AC_AttControl: add get_throttle_in accessorUsed for logging only " _throttle_in = throttle_in _throttle_in = throttle_in "
ArduPilot 281 AC_AttitudeControl.cpp Control 979534279a AC_AttControl: roll pitch yaw output to motors in -1 to +1 range " float output = _pid_rate_roll.get_p + integrator + _pid_rate_roll.get_d / 4500.0f return constrain_float output -1.0f 1.0f float output = _pid_rate_pitch.get_p + integrator + _pid_rate_pitch.get_d / 4500.0f return constrain_float output -1.0f 1.0f float output = _pid_rate_yaw.get_p + integrator + _pid_rate_yaw.get_d / 4500.0f return constrain_float output -1.0f 1.0f "
ArduPilot 282 AC_AttitudeControl.cpp Control 17c9db08f3 AC_AttControl: add angle and rate PIDs " AP_SUBGROUPINFO _p_angle_roll ANG_RLL_ 13 AC_AttitudeControl AC_P AP_SUBGROUPINFO _p_angle_pitch ANG_PIT_ 14 AC_AttitudeControl AC_P AP_SUBGROUPINFO _p_angle_yaw ANG_YAW_ 15 AC_AttitudeControl AC_P get_rate_roll_pid .reset_I get_rate_pitch_pid .reset_I get_rate_yaw_pid .reset_I get_rate_roll_pid .set_input_filter_d degrees rate_error_rads *100.0f get_rate_roll_pid .set_desired_rate degrees rate_target_rads *100.0f float integrator = get_rate_roll_pid .get_integrator integrator = get_rate_roll_pid .get_i float output = get_rate_roll_pid .get_p + integrator + get_rate_roll_pid .get_d / 4500.0f get_rate_pitch_pid .set_input_filter_d degrees rate_error_rads *100.0f get_rate_pitch_pid .set_desired_rate degrees rate_target_rads *100.0f float integrator = get_rate_pitch_pid .get_integrator integrator = get_rate_pitch_pid .get_i float output = get_rate_pitch_pid .get_p + integrator + get_rate_pitch_pid .get_d / 4500.0f get_rate_yaw_pid .set_input_filter_all degrees rate_error_rads *100.0f get_rate_yaw_pid .set_desired_rate degrees rate_target_rads *100.0f float integrator = get_rate_yaw_pid .get_integrator integrator = get_rate_yaw_pid .get_i float output = get_rate_yaw_pid .get_p + integrator + get_rate_yaw_pid .get_d / 4500.0f float alpha = get_rate_roll_pid .get_filt_alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid .kD /_dt + get_rate_roll_pid .kP float alpha = get_rate_pitch_pid .get_filt_alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid .kD /_dt + get_rate_pitch_pid .kP float alpha = get_rate_yaw_pid .get_filt_alpha return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid .kD /_dt + get_rate_yaw_pid .kP "
ArduPilot 283 AC_AttitudeControl.cpp Non-Control b30606bb22 AC_AttControl: remove scaling for centi-degrees and legacy motor input " get_rate_roll_pid .set_input_filter_d rate_error_rads get_rate_roll_pid .set_desired_rate rate_target_rads float output = get_rate_roll_pid .get_p + integrator + get_rate_roll_pid .get_d get_rate_pitch_pid .set_input_filter_d rate_error_rads get_rate_pitch_pid .set_desired_rate rate_target_rads float output = get_rate_pitch_pid .get_p + integrator + get_rate_pitch_pid .get_d get_rate_yaw_pid .set_input_filter_all rate_error_rads get_rate_yaw_pid .set_desired_rate rate_target_rads float output = get_rate_yaw_pid .get_p + integrator + get_rate_yaw_pid .get_d "
ArduPilot 284 AC_AttitudeControl.cpp Non-Control 4a06ca4be2 AC_AttControl: remove unused call to motors.set_stabilizingAlso minor change to order of a call to motors library to make stabilizingand non-stabilizing calls consistent.Non functional change " _motors.set_throttle_filter_cutoff filter_cutoff "
ArduPilot 285 AC_AttitudeControl.cpp Non-Control 6330060e49 AC_AttitudeControl: added monitoring of controller erroruse RMS P+I+FF output. Thanks to Leonard for the suggestion " control_monitor_update "
ArduPilot 286 AC_AttitudeControl.cpp Non-Control 35ef761deb AC_AttitudeControl: Move set_throttle_out to _Multi and _Heli ""
ArduPilot 287 AC_AttitudeControl.cpp Control b7431b7d0c AC_AttitudeControl: update throttle rpy mix on every iteration " update_throttle_rpy_mix "
ArduPilot 288 AC_AttitudeControl.cpp Non-Control 7ff0fcb25d AC_AttitudeControl: multicopter specific rate_controller_run ""
ArduPilot 289 AC_AttitudeControl.cpp Control 3d27ecca92 AC_AttitudeControl: add TC for Alt_Hold angle limit ""
ArduPilot 290 AC_AttitudeControl.cpp Non-Control dafc45eb26 AC_AttitudeControl: move get_althold_lean_angle_max to parent class " float AC_AttitudeControl::get_althold_lean_angle_max const return ToDeg _althold_lean_angle_max * 100.0f "
ArduPilot 291 AC_AttitudeControl.cpp Control 096bdd67f8 AC_AttitudeControl: add ATC_ANG_LIM_TC parameterThis allow adjusting the reponse to limit lean angles to reduce altitude loss " AP_GROUPINFO ANG_LIM_TC 16 AC_AttitudeControl _angle_limit_tc AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT "
ArduPilot 292 AC_AttitudeControl.cpp Control 8737f6b62d AC_AttitudeControl: enhanced quaternion attitude controller " void AC_AttitudeControl::set_throttle_out_unstabilized float throttle_in bool reset_attitude_control float filter_cutoff _throttle_in = throttle_in _motors.set_throttle_filter_cutoff filter_cutoff if reset_attitude_control relax_attitude_controllers _motors.set_throttle throttle_in _angle_boost = 0.0f void AC_AttitudeControl::relax_attitude_controllers _attitude_target_quat.from_rotation_matrix _ahrs.get_rotation_body_to_ned _attitude_target_ang_vel = _ahrs.get_gyro _attitude_target_euler_angle = Vector3f _ahrs.roll _ahrs.pitch _ahrs.yaw _rate_target_ang_vel = _ahrs.get_gyro void AC_AttitudeControl::input_quaternion Quaternion attitude_desired_quat float smoothing_gain _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z smoothing_gain = constrain_float smoothing_gain 1.0f 1/_dt Quaternion attitude_error_quat = _attitude_target_quat.inverse * attitude_desired_quat Vector3f attitude_error_angle attitude_error_quat.to_axis_angle attitude_error_angle if _rate_bf_ff_enabled & _use_ff_and_input_shaping _attitude_target_ang_vel.x = input_shaping_angle attitude_error_angle.x smoothing_gain get_accel_roll_max_radss _attitude_target_ang_vel.x _attitude_target_ang_vel.y = input_shaping_angle attitude_error_angle.y smoothing_gain get_accel_pitch_max_radss _attitude_target_ang_vel.y _attitude_target_ang_vel.z = input_shaping_angle attitude_error_angle.z smoothing_gain get_accel_yaw_max_radss _attitude_target_ang_vel.z ang_vel_to_euler_rate _attitude_target_euler_angle _attitude_target_ang_vel _attitude_target_euler_rate else _attitude_target_quat = attitude_desired_quat _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f attitude_controller_run_quat void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_rate_cds float smoothing_gain float euler_roll_angle = radians euler_roll_angle_cd*0.01f float euler_pitch_angle = radians euler_pitch_angle_cd*0.01f float euler_yaw_rate = radians euler_yaw_rate_cds*0.01f _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z smoothing_gain = constrain_float smoothing_gain 1.0f 1/_dt euler_roll_angle += get_roll_trim_rad if _rate_bf_ff_enabled & _use_ff_and_input_shaping Vector3f euler_accel = euler_accel_limit _attitude_target_euler_angle Vector3f get_accel_roll_max_radss get_accel_pitch_max_radss get_accel_yaw_max_radss _attitude_target_euler_rate.x = input_shaping_angle euler_roll_angle-_attitude_target_euler_angle.x smoothing_gain euler_accel.x _attitude_target_euler_rate.x _attitude_target_euler_rate.y = input_shaping_angle euler_pitch_angle-_attitude_target_euler_angle.y smoothing_gain euler_accel.y _attitude_target_euler_rate.y _attitude_target_euler_rate.z = input_shaping_ang_vel _attitude_target_euler_rate.z euler_yaw_rate euler_accel.z euler_rate_to_ang_vel _attitude_target_euler_angle _attitude_target_euler_rate _attitude_target_ang_vel _attitude_target_euler_angle.x = euler_roll_angle _attitude_target_euler_angle.y = euler_pitch_angle _attitude_target_euler_angle.z += euler_yaw_rate*_dt _attitude_target_quat.from_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f attitude_controller_run_quat void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw float euler_roll_angle_cd float euler_pitch_angle_cd float euler_yaw_angle_cd bool slew_yaw float smoothing_gain float euler_roll_angle = radians euler_roll_angle_cd*0.01f float euler_pitch_angle = radians euler_pitch_angle_cd*0.01f float euler_yaw_angle = radians euler_yaw_angle_cd*0.01f _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z smoothing_gain = constrain_float smoothing_gain 1.0f 1/_dt euler_roll_angle += get_roll_trim_rad if _rate_bf_ff_enabled & _use_ff_and_input_shaping Vector3f euler_accel = euler_accel_limit _attitude_target_euler_angle Vector3f get_accel_roll_max_radss get_accel_pitch_max_radss get_accel_yaw_max_radss _attitude_target_euler_rate.x = input_shaping_angle euler_roll_angle-_attitude_target_euler_angle.x smoothing_gain euler_accel.x _attitude_target_euler_rate.x _attitude_target_euler_rate.y = input_shaping_angle euler_pitch_angle-_attitude_target_euler_angle.y smoothing_gain euler_accel.y _attitude_target_euler_rate.y _attitude_target_euler_rate.z = input_shaping_angle euler_yaw_angle-_attitude_target_euler_angle.z smoothing_gain euler_accel.z _attitude_target_euler_rate.z if slew_yaw _attitude_target_euler_rate.z = constrain_float _attitude_target_euler_rate.z -get_slew_yaw_rads get_slew_yaw_rads euler_rate_to_ang_vel _attitude_target_euler_angle _attitude_target_euler_rate _attitude_target_ang_vel _attitude_target_euler_angle.x = euler_roll_angle _attitude_target_euler_angle.y = euler_pitch_angle if slew_yaw float angle_error = constrain_float wrap_PI euler_yaw_angle-_attitude_target_euler_angle.z -get_slew_yaw_rads *_dt get_slew_yaw_rads *_dt _attitude_target_euler_angle.z = wrap_PI angle_error + _attitude_target_euler_angle.z else _attitude_target_euler_angle.z = euler_yaw_angle _attitude_target_quat.from_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f attitude_controller_run_quat void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw float euler_roll_rate_cds float euler_pitch_rate_cds float euler_yaw_rate_cds float euler_roll_rate = radians euler_roll_rate_cds*0.01f float euler_pitch_rate = radians euler_pitch_rate_cds*0.01f float euler_yaw_rate = radians euler_yaw_rate_cds*0.01f _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z if _rate_bf_ff_enabled & _use_ff_and_input_shaping Vector3f euler_accel = euler_accel_limit _attitude_target_euler_angle Vector3f get_accel_roll_max_radss get_accel_pitch_max_radss get_accel_yaw_max_radss _attitude_target_euler_rate.x = input_shaping_ang_vel _attitude_target_euler_rate.x euler_roll_rate euler_accel.x _attitude_target_euler_rate.y = input_shaping_ang_vel _attitude_target_euler_rate.y euler_pitch_rate euler_accel.y _attitude_target_euler_rate.z = input_shaping_ang_vel _attitude_target_euler_rate.z euler_yaw_rate euler_accel.z euler_rate_to_ang_vel _attitude_target_euler_angle _attitude_target_euler_rate _attitude_target_ang_vel else _attitude_target_euler_angle.x = wrap_PI _attitude_target_euler_angle.x + euler_roll_rate*_dt _attitude_target_euler_angle.y = constrain_float _attitude_target_euler_angle.y + euler_pitch_rate*_dt radians -85.0f radians 85.0f _attitude_target_euler_angle.z = wrap_2PI _attitude_target_euler_angle.z + euler_yaw_rate*_dt _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f _attitude_target_quat.from_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z attitude_controller_run_quat void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw float roll_rate_bf_cds float pitch_rate_bf_cds float yaw_rate_bf_cds float roll_rate_rads = radians roll_rate_bf_cds*0.01f float pitch_rate_rads = radians pitch_rate_bf_cds*0.01f float yaw_rate_rads = radians yaw_rate_bf_cds*0.01f _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z if _rate_bf_ff_enabled & _use_ff_and_input_shaping _attitude_target_ang_vel.x = input_shaping_ang_vel _attitude_target_ang_vel.x roll_rate_rads get_accel_roll_max_radss _attitude_target_ang_vel.y = input_shaping_ang_vel _attitude_target_ang_vel.y pitch_rate_rads get_accel_pitch_max_radss _attitude_target_ang_vel.z = input_shaping_ang_vel _attitude_target_ang_vel.z yaw_rate_rads get_accel_yaw_max_radss ang_vel_to_euler_rate _attitude_target_euler_angle _attitude_target_ang_vel _attitude_target_euler_rate Quaternion attitude_target_update_quat attitude_target_update_quat.from_axis_angle Vector3f roll_rate_rads * _dt pitch_rate_rads * _dt yaw_rate_rads * _dt _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat _attitude_target_quat.normalize _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f attitude_controller_run_quat void AC_AttitudeControl::attitude_controller_run_quat Quaternion attitude_vehicle_quat attitude_vehicle_quat.from_rotation_matrix _ahrs.get_rotation_body_to_ned Vector3f attitude_error_vector thrust_heading_rotation_angles _attitude_target_quat attitude_vehicle_quat attitude_error_vector _thrust_error_angle _rate_target_ang_vel = update_ang_vel_target_from_att_error attitude_error_vector _rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro .z _rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro .z Quaternion attitude_target_ang_vel_quat = Quaternion 0.0f _attitude_target_ang_vel.x _attitude_target_ang_vel.y _attitude_target_ang_vel.z Quaternion attitude_error_quat = attitude_vehicle_quat.inverse * _attitude_target_quat Quaternion target_ang_vel_quat = attitude_error_quat.inverse *attitude_target_ang_vel_quat*attitude_error_quat if _thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f _rate_target_ang_vel.z = _ahrs.get_gyro .z else if _thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE float flip_scalar = 1.0f - _thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE /AC_ATTITUDE_THRUST_ERROR_ANGLE _rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar _rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar _rate_target_ang_vel.z += target_ang_vel_quat.q4 _rate_target_ang_vel.z = _ahrs.get_gyro .z* 1.0-flip_scalar + _rate_target_ang_vel.z*flip_scalar _rate_target_ang_vel.x += target_ang_vel_quat.q2 _rate_target_ang_vel.y += target_ang_vel_quat.q3 _rate_target_ang_vel.z += target_ang_vel_quat.q4 if _rate_bf_ff_enabled & _use_ff_and_input_shaping Quaternion attitude_target_update_quat attitude_target_update_quat.from_axis_angle Vector3f _attitude_target_ang_vel.x * _dt _attitude_target_ang_vel.y * _dt _attitude_target_ang_vel.z * _dt _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat _attitude_target_quat.normalize void AC_AttitudeControl::thrust_heading_rotation_angles Quaternion& att_to_quat const Quaternion& att_from_quat Vector3f& att_diff_angle float& thrust_vec_dot Matrix3f att_to_rot_matrix att_to_quat.rotation_matrix att_to_rot_matrix Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f 0.0f 0.0f 1.0f Matrix3f att_from_rot_matrix att_from_quat.rotation_matrix att_from_rot_matrix Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f 0.0f 0.0f 1.0f Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec thrust_vec_dot = acosf constrain_float att_from_thrust_vec * att_to_thrust_vec -1.0f 1.0f float thrust_vector_length = thrust_vec_cross.length if is_zero thrust_vector_length || is_zero thrust_vec_dot thrust_vec_cross = Vector3f 0 0 1 thrust_vec_dot = 0.0f else thrust_vec_cross /= thrust_vector_length Quaternion thrust_vec_correction_quat thrust_vec_correction_quat.from_axis_angle thrust_vec_cross thrust_vec_dot thrust_vec_correction_quat = att_from_quat.inverse *thrust_vec_correction_quat*att_from_quat Quaternion heading_quat = thrust_vec_correction_quat.inverse *att_from_quat.inverse *att_to_quat Vector3f rotation thrust_vec_correction_quat.to_axis_angle rotation att_diff_angle.x = rotation.x att_diff_angle.y = rotation.y heading_quat.to_axis_angle rotation att_diff_angle.z = rotation.z if !is_zero _p_angle_yaw.kP && fabsf att_diff_angle.z > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP att_diff_angle.z = constrain_float wrap_PI att_diff_angle.z -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP heading_quat.from_axis_angle Vector3f 0.0f 0.0f att_diff_angle.z att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat float AC_AttitudeControl::input_shaping_angle float error_angle float smoothing_gain float accel_max float target_ang_vel error_angle = wrap_PI error_angle float ang_vel = sqrt_controller error_angle smoothing_gain accel_max float delta_ang_vel = accel_max * _dt return constrain_float ang_vel target_ang_vel-delta_ang_vel target_ang_vel+delta_ang_vel float AC_AttitudeControl::input_shaping_ang_vel float target_ang_vel float desired_ang_vel float accel_max if accel_max > 0.0f float delta_ang_vel = accel_max * _dt target_ang_vel += constrain_float desired_ang_vel - target_ang_vel -delta_ang_vel delta_ang_vel else target_ang_vel = desired_ang_vel return target_ang_vel Vector3f AC_AttitudeControl::euler_accel_limit Vector3f euler_rad Vector3f euler_accel float sin_phi = constrain_float fabsf sinf euler_rad.x 0.1f 1.0f float cos_phi = constrain_float fabsf cosf euler_rad.x 0.1f 1.0f float sin_theta = constrain_float fabsf sinf euler_rad.y 0.1f 1.0f Vector3f rot_accel if is_zero euler_accel.x || is_zero euler_accel.y || is_zero euler_accel.z || euler_accel.x < 0.0f || euler_accel.y < 0.0f || euler_accel.z < 0.0f rot_accel.x = euler_accel.x rot_accel.y = euler_accel.y rot_accel.z = euler_accel.z else rot_accel.x = euler_accel.x rot_accel.y = MIN euler_accel.y/cos_phi euler_accel.z/sin_phi rot_accel.z = MIN MIN euler_accel.x/sin_theta euler_accel.y/sin_phi euler_accel.z/cos_phi return rot_accel void AC_AttitudeControl::shift_ef_yaw_target float yaw_shift_cd float yaw_shift = radians yaw_shift_cd*0.01f _attitude_target_euler_angle.z = wrap_2PI _attitude_target_euler_angle.z + yaw_shift Quaternion _attitude_target_update_quat _attitude_target_update_quat.from_axis_angle Vector3f 0.0f 0.0f _attitude_target_euler_angle.z _attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error Vector3f attitude_error_rot_vec_rad Vector3f rate_target_ang_vel if _use_ff_and_input_shaping rate_target_ang_vel.x = sqrt_controller attitude_error_rot_vec_rad.x _p_angle_roll.kP constrain_float get_accel_roll_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS rate_target_ang_vel.x = _p_angle_roll.kP * attitude_error_rot_vec_rad.x if _use_ff_and_input_shaping rate_target_ang_vel.y = sqrt_controller attitude_error_rot_vec_rad.y _p_angle_pitch.kP constrain_float get_accel_pitch_max_radss /2.0f AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS rate_target_ang_vel.y = _p_angle_pitch.kP * attitude_error_rot_vec_rad.y if _use_ff_and_input_shaping rate_target_ang_vel.z = sqrt_controller attitude_error_rot_vec_rad.z _p_angle_yaw.kP constrain_float get_accel_yaw_max_radss /2.0f AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS rate_target_ang_vel.z = _p_angle_yaw.kP * attitude_error_rot_vec_rad.z return rate_target_ang_vel float AC_AttitudeControl::rate_target_to_motor_roll float rate_target_rads float AC_AttitudeControl::rate_target_to_motor_pitch float rate_target_rads float AC_AttitudeControl::rate_target_to_motor_yaw float rate_target_rads float AC_AttitudeControl::stopping_point float first_ord_mag float p float second_ord_lim if second_ord_lim > 0.0f && !is_zero second_ord_lim && is_zero p return first_ord_mag*first_ord_mag / 2.0f*second_ord_lim else if second_ord_lim < 0.0f || is_zero second_ord_lim && !is_zero p return first_ord_mag/p else if second_ord_lim < 0.0f || is_zero second_ord_lim && is_zero p return 0.0f float linear_velocity = second_ord_lim/p if fabsf first_ord_mag < linear_velocity return first_ord_mag/p else float linear_dist = second_ord_lim/sq p float overshoot = linear_dist/2.0f + sq first_ord_mag / 2.0f*second_ord_lim if first_ord_mag > 0 return overshoot else return -overshoot "
ArduPilot 293 AC_AttitudeControl.cpp Control 80bda572ba AC_AttutudeControl: Yaw shift fix " _attitude_target_update_quat.from_axis_angle Vector3f 0.0f 0.0f yaw_shift "
ArduPilot 294 AC_AttitudeControl.cpp Control 011bc0a350 AC_AttControl: add reset_rate_controller_I_terms() " void AC_AttitudeControl::reset_rate_controller_I_terms get_rate_roll_pid .reset_I get_rate_pitch_pid .reset_I get_rate_yaw_pid .reset_I "
ArduPilot 295 AC_AttitudeControl.cpp Control 29d4790fc7 AC_AttControl: reduce max_rate_step based on hover throttleThis changes reduces AutoTune s twitch size based on the hover throttleThis method is only used by AutoTune " return 2.0f*_motors.get_throttle_hover *AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid .kD /_dt + get_rate_roll_pid .kP return 2.0f*_motors.get_throttle_hover *AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid .kD /_dt + get_rate_pitch_pid .kP return 2.0f*_motors.get_throttle_hover *AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/ alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid .kD /_dt + get_rate_yaw_pid .kP "
ArduPilot 296 AC_AttitudeControl.cpp Non-Control 152edf7189 Global: remove mode line from headersUsing a global .dir-locals.el file is a better alternative thanreincluding the same emacs header in every file of the project. ""
ArduPilot 297 AC_AttitudeControl.cpp Control ea0e413b04 AC_AttControl: do not limit rate if ATC_ACCEL_MAX param is zero " if accel_max > 0 float delta_ang_vel = accel_max * _dt return constrain_float ang_vel target_ang_vel-delta_ang_vel target_ang_vel+delta_ang_vel else return ang_vel "
ArduPilot 298 AC_AttitudeControl.cpp Control 5acbf5d16e AC_AttitudeControl: use FF from AC_PID " float output = get_rate_roll_pid .get_p + integrator + get_rate_roll_pid .get_d + get_rate_roll_pid .get_ff rate_target_rads float output = get_rate_pitch_pid .get_p + integrator + get_rate_pitch_pid .get_d + get_rate_pitch_pid .get_ff rate_target_rads float output = get_rate_yaw_pid .get_p + integrator + get_rate_yaw_pid .get_d + get_rate_yaw_pid .get_ff rate_target_rads "
ArduPilot 299 AC_AttitudeControl.cpp Control 67097c8d59 AC_AttitudeControl: use gyro_latestThis allows moving the attitude control before the ahrs/ekf updateWe continue to use ahrs.get_gyro for the non-time-critical helipassthrough functions. We should avoid using get_gyro_latest() unless we know there is a direct benefit because it could lead to inconsistent gyro values vs what the EKF is using. " float AC_AttitudeControl::rate_target_to_motor_roll float rate_actual_rads float rate_target_rads float rate_error_rads = rate_target_rads - rate_actual_rads float AC_AttitudeControl::rate_target_to_motor_pitch float rate_actual_rads float rate_target_rads float rate_error_rads = rate_target_rads - rate_actual_rads float AC_AttitudeControl::rate_target_to_motor_yaw float rate_actual_rads float rate_target_rads float rate_error_rads = rate_target_rads - rate_actual_rads "
ArduPilot 300 AC_AttitudeControl.cpp Non-Control 188dfb6936 AC_AttitudeControl: Use SI units conventions in parameter unitsFollow the rules from:http://physics.nist.gov/cuu/Units/units.htmlhttp://physics.nist.gov/cuu/Units/outside.htmlandhttp://physics.nist.gov/cuu/Units/checklist.htmlone further constrain is that only printable (7bit) ASCII characters are allowed ""
ArduPilot 301 AC_AttitudeControl.cpp Control 0544cf1d82 AC_AttControl: add step input for autotune " void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw float roll_angle_step_bf_cd float pitch_angle_step_bf_cd float yaw_angle_step_bf_cd float roll_step_rads = radians roll_angle_step_bf_cd*0.01f float pitch_step_rads = radians pitch_angle_step_bf_cd*0.01f float yaw_step_rads = radians yaw_angle_step_bf_cd*0.01f Quaternion attitude_target_update_quat attitude_target_update_quat.from_axis_angle Vector3f roll_step_rads pitch_step_rads yaw_step_rads _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat _attitude_target_quat.normalize _attitude_target_quat.to_euler _attitude_target_euler_angle.x _attitude_target_euler_angle.y _attitude_target_euler_angle.z _attitude_target_euler_rate = Vector3f 0.0f 0.0f 0.0f _attitude_target_ang_vel = Vector3f 0.0f 0.0f 0.0f attitude_controller_run_quat "
ArduPilot 303 AP_Baro.cpp Control 5bccfbd94c Barometer: fixed airstart for APMwe need to re-load the ground pressure and temperature from eeprom onand airstart " const AP_Param::GroupInfo AP_Baro::var_info PROGMEM = AP_GROUPINFO ABS_PRESS 0 AP_Baro _ground_pressure AP_GROUPINFO TEMP 1 AP_Baro _ground_temperature AP_GROUPEND "
ArduPilot 304 AP_Baro.cpp Control f9fc7aafe7 AP_Baro: use DerivativeFilter in barometer climb rate " _last_climb_rate_t = _last_update _climb_rate = _climb_rate_filter.apply get_altitude "
ArduPilot 305 AP_Baro.cpp Control 185c6e5b32 AP_Baro: change to a 7 point DerivativeFilter for climb rate " _climb_rate = _climb_rate_filter.apply get_altitude _last_update * 1.0e3 "
ArduPilot 306 AP_Baro.cpp Control f501503eb0 AP_Baro: improved barometer averagingthis changes the barometer calculations to floating point. On a MS5611this is actually about twice as fast as the previous 64 bitcalculations but gains us more accuracy as we are able to takeadvantage of sub-bit precision when we average over 8 samples. " AP_GROUPINFO ABS_PRESS 2 AP_Baro _ground_pressure AP_GROUPINFO TEMP 3 AP_Baro _ground_temperature float ground_pressure = 0 float ground_temperature = 0 for uint16_t i = 0 i < 10 i++ do read while !healthy ground_pressure = get_pressure ground_temperature = get_temperature callback 100 for uint16_t i = 0 i < 5 i++ ground_pressure= ground_pressure * 0.8 + get_pressure * 0.2 ground_temperature= ground_temperature * 0.8 + get_temperature * 0.2 callback 100 _climb_rate_filter.update _altitude _last_update if _last_climb_rate_t == _last_altitude_t _last_climb_rate_t = _last_altitude_t _climb_rate = _climb_rate_filter.slope * 1.0e3 "
ArduPilot 307 AP_Baro.cpp Non-Control a7b9aff79f AP_Baro: the DerivativeFilter now handles duplicate datathis saves a few bytes of memory " return _climb_rate_filter.slope * 1.0e3 "
ArduPilot 308 AP_Baro.cpp Non-Control 518d6365ff AP_Param: update remaining libraries for new constructor syntax " AP_GROUPINFO ABS_PRESS 2 AP_Baro _ground_pressure 0 AP_GROUPINFO TEMP 3 AP_Baro _ground_temperature 0 "
ArduPilot 309 AP_Baro.cpp Non-Control 6f02a645c3 uncrustify libraries/AP_Baro/AP_Baro.cpp " * APM_Baro.cpp - barometer driver * * This library is free software you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public License * as published by the Free Software Foundation either version 2.1 * of the License or at your option any later version. */ float ground_pressure = 0 float ground_temperature = 0 while ground_pressure == 0 || !healthy read ground_pressure = get_pressure ground_temperature = get_temperature callback 20 for uint16_t i = 0 i < 10 i++ do read while !healthy ground_pressure = get_pressure ground_temperature = get_temperature callback 100 for uint16_t i = 0 i < 5 i++ do read while !healthy ground_pressure = ground_pressure * 0.8 + get_pressure * 0.2 ground_temperature = ground_temperature * 0.8 + get_temperature * 0.2 callback 100 _ground_pressure.set_and_save ground_pressure _ground_temperature.set_and_save ground_temperature / 10.0f float scaling temp scaling = float _ground_pressure / float get_pressure temp = float _ground_temperature + 273.15f _altitude = log scaling * temp * 29.271267f "
ArduPilot 310 AP_Baro.cpp Non-Control 5d40074e4e AP_Baro: port to AP_HAL " #include #include extern const AP_HAL::HAL& hal void AP_Baro::calibrate hal.scheduler->delay 20 hal.scheduler->delay 100 hal.scheduler->delay 100 "
ArduPilot 311 AP_Baro.cpp Control 60f7788be4 AP_Baro: robust calibration " uint32_t tstart = hal.scheduler->millis while ground_pressure == 0 || !healthy read if hal.scheduler->millis - tstart > 500 hal.console->println_P PSTR PANIC: AP_Baro::read unsuccessful for more than 500ms in AP_Baro::calibrate 1 \r\n return ground_pressure = get_pressure ground_temperature = get_temperature hal.scheduler->delay 20 for uint8_t i = 0 i < 10 i++ uint32_t tstart = hal.scheduler->millis if hal.scheduler->millis - tstart > 500 hal.console->println_P PSTR PANIC: AP_Baro::read unsuccessful for more than 500ms in AP_Baro::calibrate 2 \r\n return ground_pressure = get_pressure ground_temperature = get_temperature uint32_t tstart = hal.scheduler->millis if hal.scheduler->millis - tstart > 500 hal.console->println_P PSTR PANIC: AP_Baro::read unsuccessful for more than 500ms in AP_Baro::calibrate 3 \r\n return ground_pressure = ground_pressure * 0.8 + get_pressure * 0.2 ground_temperature = ground_temperature * 0.8 + get_temperature * 0.2 "
ArduPilot 312 AP_Baro.cpp Non-Control d92e8045c1 AP_Baro: uses scheduler panic " hal.scheduler->panic PSTR PANIC: AP_Baro::read unsuccessful hal.scheduler->panic PSTR PANIC: AP_Baro::read unsuccessful hal.scheduler->panic PSTR PANIC: AP_Baro::read unsuccessful "
ArduPilot 313 AP_Baro.cpp Control 5631f865b2 Update floating point calculations to use floats instead of doubles.- Allows use of hardware floating point on the Cortex-M4.- Added f suffix to floating point literals.- Call floating point versions of stdlib math functions. " ground_pressure = ground_pressure * 0.8f + get_pressure * 0.2f ground_temperature = ground_temperature * 0.8f + get_temperature * 0.2f _altitude = logf scaling * temp * 29.271267f return _climb_rate_filter.slope * 1.0e3f "
ArduPilot 314 AP_Baro.cpp Control 4fa7bb1486 Add AVR compatibility header for missing math.h definitions.- Define float versions of math functions to the double versions on AVR (eg. #define sinf sin).- These macros appear to be missing in older versions of avr-libs.- Include AP_Math.h rather than math.h to get these definitions. " #include "
ArduPilot 315 AP_Baro.cpp Non-Control aa2b10f044 AP_Baro: TEMP parameter comment fix ""
ArduPilot 316 AP_Baro.cpp Control 3b98bbd159 AP_Baro: more precise altitude calculation on PX4if not using an AVR CPU then use a more computationally expensivealtitude calculation which is more precise at higher altitudes " #if defined __AVR_ATmega1280__ || defined __AVR_ATmega2560__ #else scaling = float get_pressure / float _ground_pressure temp = float _ground_temperature + 273.15f _altitude = 153.8462f * temp * 1.0f - expf 0.190259f * logf scaling #endif"
ArduPilot 317 AP_Baro.cpp Non-Control 61361dac9b AP_Baro: added units to baro documentation ""
ArduPilot 318 AP_Baro.cpp Control 7bdb098e77 AP_Baro: added get_EAS2TAS()this provides the scaling factor between equivalent and true airspeedbased on altitude " float AP_Baro::get_EAS2TAS void if abs _altitude - _last_altitude_EAS2TAS < 100.0f && _EAS2TAS != 0.0f return _EAS2TAS float tempK = float _ground_temperature + 273.15f - 0.0065f * _altitude _EAS2TAS = safe_sqrt 1.225f / float get_pressure / 287.26f * tempK _last_altitude_EAS2TAS = _altitude return _EAS2TAS "
ArduPilot 319 AP_Baro.cpp Control 0f72401d8d AP_Baro: added GND_ALT_OFFSET parameterused for automatic barometric adjustment by a ground station equippedwith a barometer " AP_GROUPINFO ALT_OFFSET 4 AP_Baro _alt_offset 0 _alt_offset.set_and_save 0 return _altitude + _alt_offset return _altitude + _alt_offset "
ArduPilot 320 AP_Baro.cpp Control 5607c89df0 AP_Baro: use fabs() not abs()thanks to Randy for spotting this " if fabs _altitude - _last_altitude_EAS2TAS < 100.0f && _EAS2TAS != 0.0f "
ArduPilot 321 AP_Baro.cpp Non-Control 97b7130bb9 libraries: update license header to GPLv3we switched to GPLv3 a long time ago but neglected to update theper-file license headers " "
ArduPilot 322 AP_Baro.cpp Control fc119d9b80 AP_Baro: cleaned up temperature and pressure unitsthanks to Mike McCauley for pointing this out " _ground_temperature.set_and_save ground_temperature "
ArduPilot 323 AP_Baro.cpp Control 487caea3a5 AP_Baro: added update_calibration() methodused for auto baro calibration when disarmed " void AP_Baro::update_calibration _ground_pressure.set get_pressure _ground_temperature.set get_temperature "
ArduPilot 324 AP_Baro.cpp Control 5e3c4441dc AP_Baro: use HAL_CPU_CLASS in baro driver " #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16"
ArduPilot 325 AP_Baro.cpp Control 2cd781997f AP_Baro: changed HIL driver to use floats and better handle starup " if _ground_pressure == 0 return 0 "
ArduPilot 326 AP_Baro.cpp Control 41dd280739 AP_Baro: expose get_altitude_difference() this is useful for pressure altitude calculations " float AP_Baro::get_altitude_difference float base_pressure float pressure float ret #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16 float scaling = base_pressure / pressure float temp = _ground_temperature + 273.15f ret = logf scaling * temp * 29.271267f #else float scaling = pressure / base_pressure float temp = _ground_temperature + 273.15f ret = 153.8462f * temp * 1.0f - expf 0.190259f * logf scaling #endif return ret _altitude = get_altitude_difference _ground_pressure get_pressure "
ArduPilot 327 AP_Baro.cpp Control 74c3b404ee AP_Baro: avoid some float conversion warnings " if fabsf _altitude - _last_altitude_EAS2TAS < 100.0f && _EAS2TAS != 0.0f "
ArduPilot 328 AP_Baro.cpp Non-Control a458f1bf5f AP_Baro: minor improvements to Baro HIL code health check " float pressure = get_pressure _ground_pressure.set pressure float pressure = get_pressure _altitude = get_altitude_difference _ground_pressure pressure "
ArduPilot 329 AP_Baro.cpp Non-Control e9fbea9497 Baro: make get_altitude_difference const " float AP_Baro::get_altitude_difference float base_pressure float pressure const"
ArduPilot 330 AP_Baro.cpp Control 330d883f97 Baro: add altitude sanity checkhealthy flag made protectedhealthy accessor fn added which also check latest calculated altitudewas ok " while ground_pressure == 0 || !_flags.healthy while !_flags.healthy while !_flags.healthy float alt = get_altitude_difference _ground_pressure pressure if isnan alt || isinf alt _flags.alt_ok = false else _altitude = alt _flags.alt_ok = true "
ArduPilot 331 AP_Baro.cpp Non-Control 3a81732721 Baro: minor param description updates ""
ArduPilot 332 AP_Baro.cpp Control d404cc6542 AP_Baro: add set_external_temperature()this allows the use of an external temperature sensor for calibrationpurposes such as the sensor built in to the digital airspeed sensor.The main affect this has is on the EAS2TAS calculationThe get_calibration_temperature() is used to choose either an externaltemperature or an internal one. If an internal one is used then it isclamped at no higher than 25 degrees C to prevent hot electronicson startup affecting altitude scaling and EAS2TAS " ground_temperature = get_calibration_temperature ground_temperature = get_calibration_temperature get_calibration_temperature * 0.2f float last_temperature = _ground_temperature _ground_temperature.set get_calibration_temperature if fabsf last_temperature - _ground_temperature > 3 _EAS2TAS = 0 void AP_Baro::set_external_temperature float temperature _external_temperature = temperature _last_external_temperature_ms = hal.scheduler->millis float AP_Baro::get_calibration_temperature void const if _last_external_temperature_ms != 0 && hal.scheduler->millis - _last_external_temperature_ms < 10000 return _external_temperature float ret = get_temperature if ret > 25 ret = 25 return ret "
ArduPilot 333 AP_Baro.cpp Control 5bb57a31f7 AP_Baro: split into frontend/backendthis allows for support of multiple sensors on a board " AP_GROUPINFO ABS_PRESS 2 AP_Baro sensors 0 .ground_pressure 0 AP_GROUPINFO TEMP 3 AP_Baro sensors 0 .ground_temperature 0 AP_Baro::AP_Baro : _last_altitude_EAS2TAS 0.0f _EAS2TAS 0.0f _num_sensors 0 _num_drivers 0 memset sensors 0 sizeof sensors AP_Param::setup_object_defaults this var_info update while !healthy float sum_pressure BARO_MAX_INSTANCES = 0 float sum_temperature BARO_MAX_INSTANCES = 0 const uint8_t num_samples = 5 for uint8_t c = 0 c < num_samples c++ bool all_healthy = false update all_healthy = true for uint8_t i=0 i<_num_sensors i++ if !healthy i all_healthy = false while !all_healthy for uint8_t i=0 i<_num_sensors i++ sum_pressure i += sensors i .pressure sum_temperature i += sensors i .temperature for uint8_t i=0 i<_num_sensors i++ sensors i .ground_pressure.set_and_save sum_pressure i / num_samples sensors i .ground_temperature.set_and_save sum_temperature i / num_samples void AP_Baro::init void #if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN drivers 0 = new AP_Baro_PX4 *this _num_drivers = 1 #elif HAL_BARO_DEFAULT == HAL_BARO_HIL drivers 0 = new AP_Baro_HIL *this _num_drivers = 1 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 drivers 0 = new AP_Baro_MS5611 *this new AP_SerialBus_I2C MS5611_I2C_ADDR _num_drivers = 1 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI drivers 0 = new AP_Baro_MS5611 *this new AP_SerialBus_SPI AP_HAL::SPIDevice_MS5611 AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH _num_drivers = 1 #endif if _num_drivers == 0 || _num_sensors == 0 || drivers 0 == NULL hal.scheduler->panic PSTR Baro: unable to initialise driver void AP_Baro::update void for uint8_t i=0 i<_num_drivers i++ drivers i ->update uint32_t now = hal.scheduler->millis for uint8_t i=0 i<_num_sensors i++ sensors i .healthy = now - sensors i .last_update_ms < 500 && sensors i .pressure != 0 for uint8_t i=0 i<_num_sensors i++ if sensors i .healthy if sensors i .ground_pressure == 0 sensors i .ground_pressure = sensors i .pressure sensors i .altitude = get_altitude_difference sensors i .ground_pressure sensors i .pressure sensors i .alt_ok = ! isnan sensors i .altitude || isinf sensors i .altitude _primary = 0 for uint8_t i=0 i<_num_sensors i++ if healthy i _primary = i break _climb_rate_filter.update get_altitude get_last_update void AP_Baro::accumulate void for uint8_t i=0 i<_num_drivers i++ drivers i ->accumulate uint8_t AP_Baro::register_sensor void if _num_sensors >= BARO_MAX_INSTANCES hal.scheduler->panic PSTR Too many barometers return _num_sensors++ "
ArduPilot 334 AP_Baro.cpp Control f1891cea1f AP_Baro: BMP085 driver done but untested " #elif HAL_BARO_DEFAULT == HAL_BARO_BMP085 drivers 0 = new AP_Baro_BMP085 *this _num_drivers = 1 "
ArduPilot 335 AP_Baro.cpp Non-Control 8359c082ca AP_Baro: fixed baro on NavIO don t use the 1kHz timer as it conflicts with other I2C devices " drivers 0 = new AP_Baro_MS5611 *this new AP_SerialBus_I2C MS5611_I2C_ADDR false AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH true "
ArduPilot 336 AP_Baro.cpp Control 1c2a6deaaf AP_Baro: added all_healthy() method " bool AP_Baro::all_healthy void const for uint8_t i=0 i<_num_sensors i++ if !healthy i return false return _num_sensors > 0 "
ArduPilot 337 AP_Baro.cpp Non-Control 0a8677b3e2 AP_Baro: fixed build after rebase with all_healthy() " bool all_sensors_healthy = false all_sensors_healthy = true all_sensors_healthy = false while !all_sensors_healthy "
ArduPilot 338 AP_Baro.cpp Control b1d8df3d54 AP_Baro: only allow calibrated sensors to be used " for uint8_t i=0 i<_num_sensors i++ sensors i .calibrated = true sensors i .alt_ok = true uint8_t count BARO_MAX_INSTANCES = 0 while !healthy if healthy i sum_pressure i += sensors i .pressure sum_temperature i += sensors i .temperature count i += 1 if count i == 0 sensors i .calibrated = false else sensors i .ground_pressure.set_and_save sum_pressure i / count i sensors i .ground_temperature.set_and_save sum_temperature i / count i for uint8_t i=0 i<_num_sensors i++ if sensors i .calibrated return hal.scheduler->panic PSTR AP_Baro: all sensors uncalibrated "
ArduPilot 339 AP_Baro.cpp Control c93e7a69a7 Baro: init external_temperature in constructor " _num_drivers 0 _num_sensors 0 _primary 0 _external_temperature 0.0f _last_external_temperature_ms 0 "
ArduPilot 340 AP_Baro.cpp Control ac3dd87790 AP_Baro: added hil_mode support " _last_external_temperature_ms 0 _hil_mode false if !_hil_mode for uint8_t i=0 i<_num_drivers i++ drivers i ->update "
ArduPilot 341 AP_Baro.cpp Control 1f70b34cbc AP_Baro: fixed baro startup on PXF cape " hal.scheduler->delay 10 "
ArduPilot 342 AP_Baro.cpp Non-Control 2e9d2e6449 AP_Baro: load only HIL backend for hil_mode " if _hil_mode drivers 0 = new AP_Baro_HIL *this _num_drivers = 1 return "
ArduPilot 343 AP_Baro.cpp Non-Control f381ef93e8 AP_Baro: add get_air_density_ratio " float AP_Baro::get_air_density_ratio void return 1.0f/ sq get_EAS2TAS "
ArduPilot 344 AP_Baro.cpp Control 1b381b5675 Baro: get_air_density_ratio gets div-by-zero check " float eas2tas = get_EAS2TAS if eas2tas > 0.0f return 1.0f/ sq get_EAS2TAS else return 1.0f "
ArduPilot 345 AP_Baro.cpp Control 0b29848277 AP_Baro: compiler warnings: apply is_zero(float) or is_equal(float) " if fabsf altitude - _last_altitude_EAS2TAS < 100.0f && !AP_Math::is_zero _EAS2TAS sensors i .healthy = now - sensors i .last_update_ms < 500 && !AP_Math::is_zero sensors i .pressure if AP_Math::is_zero sensors i .ground_pressure "
ArduPilot 346 AP_Baro.cpp Control 48fb7f8159 AP_BARO: revert AP_Math class change " if fabsf altitude - _last_altitude_EAS2TAS < 100.0f && !is_zero _EAS2TAS sensors i .healthy = now - sensors i .last_update_ms < 500 && !is_zero sensors i .pressure if is_zero sensors i .ground_pressure "
ArduPilot 347 AP_Baro.cpp Control 23787cf695 AP_Baro: use ground_temperature instead of calibration_temperature for alt calculation " float temp = get_ground_temperature + 273.15f "
ArduPilot 348 AP_Baro.cpp Control d407737434 AP_Baro: added MS5607 support " #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 && HAL_BARO_MS5611_I2C_BUS == 0 drivers 0 = new AP_Baro_MS5611 *this new AP_SerialBus_I2C hal.i2c HAL_BARO_MS5611_I2C_ADDR false #elif HAL_BARO_DEFAULT == HAL_BARO_MS5607 && HAL_BARO_MS5607_I2C_BUS == 1 drivers 0 = new AP_Baro_MS5607 *this new AP_SerialBus_I2C hal.i2c1 HAL_BARO_MS5607_I2C_ADDR true _num_drivers = 1 "
ArduPilot 349 AP_Baro.cpp Control 5732a6a144 Baro: update climb rate only if healthy " if healthy _climb_rate_filter.update get_altitude get_last_update "
ArduPilot 350 AP_Baro.cpp Non-Control f18802bc46 AP_Baro: standardize inclusion of libaries headersThis commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to so the notation #include is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source then we use the notation #include <> with the path relative to libraries folder.Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time. " #include #include #include AP_Baro.h #include "
ArduPilot 351 AP_Baro.cpp Control 749c0c190f AP_Baro - fix BARO_ALT_OFFSET paramThis param seems to have been un-implemented. This is putting it back in. Adds a meter offset to the calculated altitude form the baro sensors.Also changes it from int8 to float " AP_GROUPINFO ALT_OFFSET 5 AP_Baro _alt_offset 0 float altitude = get_altitude_difference sensors i .ground_pressure sensors i .pressure sensors i .alt_ok = ! isnan altitude || isinf altitude if sensors i .alt_ok sensors i .altitude = altitude + _alt_offset "
ArduPilot 352 AP_Baro.cpp Control a5462fec0b AP_Baro: allow selection of primary barometer and add 3rd barothis is useful for external I2C barometers on a PH2 " AP_GROUPINFO PRIMARY 6 AP_Baro _primary_baro 0 if _primary_baro >= 0 && _primary_baro < _num_sensors && healthy _primary_baro _primary = _primary_baro else _primary = 0 for uint8_t i=0 i<_num_sensors i++ if healthy i _primary = i break "
ArduPilot 353 AP_Baro.cpp Control 9a98eb35fc AP_Baro: Add MS5637 over I2C to init() " #elif HAL_BARO_DEFAULT == HAL_BARO_MS5637_I2C AP_SerialBus *bus = new AP_SerialBus_I2C HAL_BARO_MS5611_I2C_POINTER HAL_BARO_MS5611_I2C_ADDR drivers 0 = new AP_Baro_MS5637 *this bus true _num_drivers = 1 #endif"
ArduPilot 354 AP_Baro.cpp Non-Control c53e9d1ff0 AP_Baro: Fix typo " call accumulate on all drivers"
ArduPilot 355 AP_Baro.cpp Control 386547427d AP_Baro: support MS5611 on second i2c bus " #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 && HAL_BARO_MS5611_I2C_BUS == 1 drivers 0 = new AP_Baro_MS5611 *this new AP_SerialBus_I2C hal.i2c1 HAL_BARO_MS5611_I2C_ADDR false _num_drivers = 1 "
ArduPilot 356 AP_Baro.cpp Non-Control 2c38e31c93 Remove use of PSTRThe PSTR is already define as a NOP for all supported platforms. It sonly needed for AVR so here we remove all the uses throughout thecodebase.This was automated with a simple python script so it also convertsplaces which spans to multiple lines removing the matching parentheses.AVR-specific places were not changed. " hal.scheduler->panic PANIC: AP_Baro::read unsuccessful for more than 500ms in AP_Baro::calibrate 2 \r\n hal.scheduler->panic PANIC: AP_Baro::read unsuccessful for more than 500ms in AP_Baro::calibrate 3 \r\n hal.scheduler->panic AP_Baro: all sensors uncalibrated hal.scheduler->panic Baro: unable to initialise driver hal.scheduler->panic Too many barometers "
ArduPilot 357 AP_Baro.cpp Non-Control 831d8acca5 Remove use of PROGMEMNow variables don t have to be declared with PROGMEM anymore so removethem. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i s/ PROGMEM / /g git grep -l -z PROGMEM | xargs -0 sed -i s/PROGMEM//g The 2 commands were done so we don t leave behind spurious spaces.AVR-specific places were not changed. " const AP_Param::GroupInfo AP_Baro::var_info = "
ArduPilot 358 AP_Baro.cpp Control 8eef58a8c2 AP_Baro: remove check for AVR CPUsRemove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 andHAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will beremoved on separate commits. " ret = 153.8462f * temp * 1.0f - expf 0.190259f * logf scaling "
ArduPilot 359 AP_Baro.cpp Non-Control 1b13315092 AP_Baro: notify GCS of calibration " sensors i .ground_pressure.set_and_notify get_pressure i sensors i .ground_temperature.set_and_notify get_calibration_temperature i "
ArduPilot 360 AP_Baro.cpp Control 840c9e65bb AP_Baro: don t notify the GCS of new pressure reference too often " sensors i .ground_pressure.set get_pressure i sensors i .ground_temperature.set get_calibration_temperature i uint32_t now = hal.scheduler->millis if now - _last_notify_ms > 10000 sensors i .ground_pressure.notify sensors i .ground_temperature.notify _last_notify_ms = now "
ArduPilot 361 AP_Baro.cpp Control 5a280838f8 AP_Baro: use millis/micros/panic functions " uint32_t tstart = AP_HAL::millis if AP_HAL::millis - tstart > 500 AP_HAL::panic PANIC: AP_Baro::read unsuccessful uint32_t tstart = AP_HAL::millis if AP_HAL::millis - tstart > 500 AP_HAL::panic PANIC: AP_Baro::read unsuccessful AP_HAL::panic AP_Baro: all sensors uncalibrated uint32_t now = AP_HAL::millis _last_external_temperature_ms = AP_HAL::millis if _last_external_temperature_ms != 0 && AP_HAL::millis - _last_external_temperature_ms < 10000 AP_HAL::panic Baro: unable to initialise driver uint32_t now = AP_HAL::millis AP_HAL::panic Too many barometers "
ArduPilot 362 AP_Baro.cpp Non-Control 81a298c9c8 AP_Baro: reduce header scopeWe don t need to expose to other libraries how each backend isimplemented. AP_Baro.h is the main header included by other libraries.Instead of including each backend in the main header move them to wherethey are needed. Additionally standardize the order and how we includethe headers.The advantages are:- Internals of each backend is not exposed outside of the library- Faster incremental builds since we don t need to recompile whoever includes AP_Baro.h because a backend changed " #include AP_Baro.h #include #include AP_Baro_BMP085.h #include AP_Baro_HIL.h #include AP_Baro_MS5611.h #include AP_Baro_PX4.h"
ArduPilot 363 AP_Baro.cpp Control 1f4e503d91 AP_Baro: added qflight driver " #include AP_Baro_qflight.h #elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT drivers 0 = new AP_Baro_QFLIGHT *this _num_drivers = 1 "
ArduPilot 364 AP_Baro.cpp Control ff44a63007 AP_Baro: added QURT driver " #include AP_Baro_QURT.h #elif HAL_BARO_DEFAULT == HAL_BARO_QURT drivers 0 = new AP_Baro_QURT *this _num_drivers = 1 "
ArduPilot 365 AP_Baro.cpp Control f435c5ee50 AP_Baro: tolerate GND_ALT_OFFSET step inputslarge inputs make the EKF angry this will allow the param update to slew over a few seconds " if fabsf _alt_offset - _alt_offset_active > 0.1f _alt_offset_active = 0.9f*_alt_offset_active + 0.1f*_alt_offset else _alt_offset_active = _alt_offset sensors i .altitude = altitude + _alt_offset_active "
ArduPilot 366 AP_Baro.cpp Control b05954660a AP_Baro: BMP085: use I2CDevice interface " drivers 0 = new AP_Baro_BMP085 *this hal.i2c_mgr->get_device HAL_BARO_BMP085_BUS HAL_BARO_BMP085_I2C_ADDR _num_drivers = 1 "
ArduPilot 367 AP_Baro.cpp Control f1ade970a3 AP_Baro: MS5611: Use AP_HAL::Device abstractionThis allows to share almost all the I2C/SPI code and remove theAP_Serial abstraction since that is now handled by AP_HAL itself. " #include return ret std::move hal.i2c_mgr->get_device HAL_BARO_BMP085_BUS HAL_BARO_BMP085_I2C_ADDR _num_drivers = 1 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C drivers 0 = new AP_Baro_MS5611 *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5611_I2C_BUS HAL_BARO_MS5611_I2C_ADDR HAL_BARO_MS5611_USE_TIMER drivers 0 = new AP_Baro_MS5611 *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME true _num_drivers = 1 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5607_I2C drivers 0 = new AP_Baro_MS5607 *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5607_I2C_BUS HAL_BARO_MS5607_I2C_ADDR true _num_drivers = 1 drivers 0 = new AP_Baro_MS5637 *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5637_I2C_BUS HAL_BARO_MS5637_I2C_ADDR true _num_drivers = 1 drivers 0 = new AP_Baro_QFLIGHT *this _num_drivers = 1 drivers 0 = new AP_Baro_QURT *this _num_drivers = 1 "
ArduPilot 368 AP_Baro.cpp Control 0a72c2bbd5 AP_Baro: prevent bad ground pressure from making a board unbootable " float ground_pressure = sensors i .ground_pressure if is_zero ground_pressure || isnan ground_pressure || isinf ground_pressure "
ArduPilot 369 AP_Baro.cpp Non-Control cae7ea0d13 Add @Volatile @ReadOnly support ""
ArduPilot 370 AP_Baro.cpp Non-Control 0b71652afc AP_Baro; allow for exact replay of baro data " if _hil.have_crate return _hil.climb_rate if _hil.have_alt sensors 0 .altitude = _hil.altitude "
ArduPilot 371 AP_Baro.cpp Control 8b7bf5cf7a AP_Baro: allow setHIL to set last update time " if _hil.have_alt if _hil.have_last_update sensors 0 .last_update_ms = _hil.last_update_ms "
ArduPilot 372 AP_Baro.cpp Control d55050e5e3 AP_Baro: slow down the LPF for slewing to a new GND_ALT_OFFSETInstead of a couple seconds make it about 15sec. This makes TECS happy by not glitching the height demand as much. When applied too fast the TECS height demand causes a large single oscillation as it chases the filter lag. " if fabsf _alt_offset - _alt_offset_active > 0.01f _alt_offset_active = 0.95f*_alt_offset_active + 0.05f*_alt_offset "
ArduPilot 373 AP_Baro.cpp Control 8d2872d3ab AP_Baro: coverity scan - variables not initialized in constructor " AP_Baro::AP_Baro "
ArduPilot 374 AP_Baro.cpp Control 1d81df8144 Revert AP_Baro: coverity scan - variables not initialized in constructor This reverts commit 8d2872d3abe1982950bb2c3a5bff47c82cfac87a. " AP_Baro::AP_Baro : _num_drivers 0 _num_sensors 0 _primary 0 _last_altitude_EAS2TAS 0.0f _EAS2TAS 0.0f _external_temperature 0.0f _last_external_temperature_ms 0 _hil_mode false "
ArduPilot 375 AP_Baro.cpp Control 84ce499a0d AP_Baro: remove zero-init in constructor " AP_Baro::AP_Baro "
ArduPilot 376 AP_Baro.cpp Control 929348ff75 AP_Baro: MS5611: always use timer threadRemove support to run MS5611 on main thread since we shouldn t be doingI2C transactions there and we are moving to thread per bus nonetheless. " std::move hal.i2c_mgr->get_device HAL_BARO_MS5611_I2C_BUS HAL_BARO_MS5611_I2C_ADDR std::move hal.spi->get_device HAL_BARO_MS5611_NAME std::move hal.i2c_mgr->get_device HAL_BARO_MS5607_I2C_BUS HAL_BARO_MS5607_I2C_ADDR std::move hal.i2c_mgr->get_device HAL_BARO_MS5637_I2C_BUS HAL_BARO_MS5637_I2C_ADDR "
ArduPilot 377 AP_Baro.cpp Non-Control 152edf7189 Global: remove mode line from headersUsing a global .dir-locals.el file is a better alternative thanreincluding the same emacs header in every file of the project. ""
ArduPilot 378 AP_Baro.cpp Non-Control dbecb1bc25 AP_Baro: add missing parameter metadata ""
ArduPilot 379 AP_Baro.cpp Control c808ee2f49 Global: To nullptr from NULL.RC_Channel: To nullptr from NULL.AC_Fence: To nullptr from NULL.AC_Avoidance: To nullptr from NULL.AC_PrecLand: To nullptr from NULL.DataFlash: To nullptr from NULL.SITL: To nullptr from NULL.GCS_MAVLink: To nullptr from NULL.DataFlash: To nullptr from NULL.AP_Compass: To nullptr from NULL.Global: To nullptr from NULL.Global: To nullptr from NULL. " if _num_drivers == 0 || _num_sensors == 0 || drivers 0 == nullptr "
ArduPilot 380 AP_Baro.cpp Control 72c2e3a4d5 AP_Baro: select in-tree vs PX4 drivers at runtime " #include if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V1 || AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V2 drivers 0 = new AP_Baro_MS5611 *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V4 drivers 0 = new AP_Baro_MS5611 *this std::move hal.spi->get_device HAL_BARO_MS5611_SPI_INT_NAME else drivers 0 = new AP_Baro_PX4 *this "
ArduPilot 381 AP_Baro.cpp Control 66026100c3 AP_Baro: enable dual baro on PH2 " _num_drivers = 1 else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V3 drivers 0 = new AP_Baro_MS5611 *this std::move hal.spi->get_device HAL_BARO_MS5611_SPI_EXT_NAME drivers 1 = new AP_Baro_MS5611 *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME _num_drivers = 2 _num_drivers = 1 _num_drivers = 1 "
ArduPilot 382 AP_Baro.cpp Control 565f1986e0 AP_Baro: fixed FMUv1 baro and enable in-tree driver for PHMINI " if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V1 #ifdef HAL_BARO_MS5611_I2C_BUS drivers 0 = new AP_Baro_MS5611 *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5611_I2C_BUS HAL_BARO_MS5611_I2C_ADDR _num_drivers = 1 #endif else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_TEST_V2 || AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PHMINI "
ArduPilot 383 AP_Baro.cpp Control b8c789cf35 AP_Baro: switch pixracer to always use in-tree drivers " else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PIXRACER "
ArduPilot 384 AP_Baro.cpp Control d716bbedee AP_Baro: use in-tree drivers for PH2SLIM " AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PHMINI || AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PH2SLIM "
ArduPilot 385 AP_Baro.cpp Control 41b132e576 AP_Baro: setup for new board PX4 IDs " if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PX4V1 else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PIXHAWK || else if AP_BoardConfig::get_board_type == AP_BoardConfig::PX4_BOARD_PIXHAWK2 "
ArduPilot 386 AP_Baro.cpp Control f2246326bb AP_Baro: added MS5611 probing and support 3 baros on Pixhawk2this adds sensor probing for barometers simplifies the MS5611 driver and adds support for the I2C external barometers on Pixhawk2 " bool AP_Baro::_add_backend AP_Baro_Backend *backend if !backend return false if _num_drivers >= BARO_MAX_DRIVERS AP_HAL::panic Too many barometer drivers drivers _num_drivers++ = backend return true #define ADD_BACKEND backend \ do _add_backend backend \ if _num_drivers == BARO_MAX_DRIVERS || \ _num_sensors == BARO_MAX_INSTANCES \ return \ \ while 0 ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5611_I2C_BUS HAL_BARO_MS5611_I2C_ADDR ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.spi->get_device HAL_BARO_MS5611_SPI_EXT_NAME ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device 0 HAL_BARO_MS5611_I2C_ADDR ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device 1 HAL_BARO_MS5611_I2C_ADDR ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.spi->get_device HAL_BARO_MS5611_SPI_INT_NAME ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5611_I2C_BUS HAL_BARO_MS5611_I2C_ADDR ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.spi->get_device HAL_BARO_MS5611_NAME ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5607_I2C_BUS HAL_BARO_MS5607_I2C_ADDR AP_Baro_MS56XX::BARO_MS5607 ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5637_I2C_BUS HAL_BARO_MS5637_I2C_ADDR AP_Baro_MS56XX::BARO_MS5637 "
ArduPilot 387 AP_Baro.cpp Control eea7758a63 AP_Baro: added GND_EXT_BUS optionthis is needed to enable probing for a MS5611 on external I2C bus. TheMS5611 looks the same as a MS5525 airspeed sensor so we can t justauto-probe. Users will need to enable external barometers " AP_GROUPINFO EXT_BUS 7 AP_Baro _ext_bus -1 if _ext_bus >= 0 ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device _ext_bus HAL_BARO_MS5611_I2C_ADDR "
ArduPilot 388 AP_Baro.cpp Non-Control 753638851e AP_Baro: Changed if statements to switch statement. " switch AP_BoardConfig::get_board_type case AP_BoardConfig::PX4_BOARD_PX4V1: break case AP_BoardConfig::PX4_BOARD_PIXHAWK: case AP_BoardConfig::PX4_BOARD_PHMINI: case AP_BoardConfig::PX4_BOARD_PH2SLIM: break case AP_BoardConfig::PX4_BOARD_PIXHAWK2: break case AP_BoardConfig::PX4_BOARD_PIXRACER: break default: break "
ArduPilot 389 AP_Baro.cpp Control 1ce5e5360a AP_Baro: Add Bosch BMP280 driver " #include AP_Baro_BMP280.h #elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_I2C ADD_BACKEND AP_Baro_BMP280::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_BMP280_BUS HAL_BARO_BMP280_I2C_ADDR #elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_SPI ADD_BACKEND AP_Baro_BMP280::probe *this std::move hal.spi->get_device HAL_BARO_BMP280_NAME "
ArduPilot 390 AP_Baro.cpp Control 613bc46592 AP_Baro: Add support for water pressure and Sub " #include AP_GROUPINFO_FRAME SPEC_GRAV 8 AP_Baro _specific_gravity 1.0 AP_PARAM_FRAME_SUB #if BARO_MAX_INSTANCES > 1 AP_GROUPINFO ABS_PRESS2 9 AP_Baro sensors 1 .ground_pressure 0 AP_GROUPINFO TEMP2 10 AP_Baro sensors 1 .ground_temperature 0 #endif #if BARO_MAX_INSTANCES > 2 AP_GROUPINFO ABS_PRESS3 11 AP_Baro sensors 2 .ground_pressure 0 AP_GROUPINFO TEMP3 12 AP_Baro sensors 2 .ground_temperature 0 #endif void AP_Baro::calibrate bool save if save sensors i .ground_pressure.set_and_save sum_pressure i / count i sensors i .ground_temperature.set_and_save sum_temperature i / count i #if APM_BUILD_TYPE APM_BUILD_ArduSub ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device _ext_bus HAL_BARO_MS5837_I2C_ADDR AP_Baro_MS56XX::BARO_MS5837 #else #endif float altitude = sensors i .altitude if sensors i .type == BARO_TYPE_AIR altitude = get_altitude_difference sensors i .ground_pressure sensors i .pressure else if sensors i .type == BARO_TYPE_WATER altitude = sensors i .ground_pressure - sensors i .pressure / 9800.0f / _specific_gravity "
ArduPilot 391 AP_Baro.cpp Non-Control fb214ef95f AP_Baro: fixed spelling in comment ""
ArduPilot 392 AP_Baro.cpp Control d5cad2ae44 AP_Baro: support AUAV21 board " case AP_BoardConfig::PX4_BOARD_AUAV21:"
ArduPilot 393 AP_Baro.cpp Control 00c2949dfb Add support for baro on aerofc " case AP_BoardConfig::PX4_BOARD_AEROFC: ADD_BACKEND AP_Baro_MS56XX::probe *this std::move hal.i2c_mgr->get_device HAL_BARO_MS5607_I2C_BUS HAL_BARO_MS5607_I2C_ADDR AP_Baro_MS56XX::BARO_MS5607 break "
ArduPilot 394 AP_Baro.cpp Control fb24a03faf AP_Baro: don t compile drivers for devices that don t use them " #ifdef HAL_BARO_MS5607_I2C_BUS #endif"
ArduPilot 395 AP_Baro.cpp Control c37209a8d5 AP_Baro: Limit ground temperature used for the altitudeFix an incorrect EAS2TAS that was double counting altitude " #define C_TO_KELVIN 273.15f #define ISA_GAS_CONSTANT 287.26f #define ISA_LAPSE_RATE 0.0065f #define INTERNAL_TEMPERATURE_CLAMP 35.0f AP_GROUPINFO TEMP 3 AP_Baro _user_ground_temperature 0 _guessed_ground_temperature = get_external_temperature _guessed_ground_temperature = get_external_temperature _EAS2TAS = 0 float temp = get_ground_temperature + C_TO_KELVIN if fabsf altitude - _last_altitude_EAS2TAS < 25.0f && !is_zero _EAS2TAS float tempK = get_ground_temperature + C_TO_KELVIN - ISA_LAPSE_RATE * altitude _EAS2TAS = safe_sqrt 1.225f / float get_pressure / ISA_GAS_CONSTANT * tempK const float eas2tas = get_EAS2TAS return 1.0f/ sq eas2tas float AP_Baro::get_ground_temperature void const if is_zero _user_ground_temperature return _guessed_ground_temperature else return _user_ground_temperature float AP_Baro::get_external_temperature const uint8_t instance const return MIN get_temperature instance INTERNAL_TEMPERATURE_CLAMP if !is_zero _user_ground_temperature _user_ground_temperature.set_and_save 0.0f _user_ground_temperature.notify "
ArduPilot 396 AP_Baro.cpp Control 331419a51e AP_Baro: support for UAVCAN connected barometers " #if HAL_WITH_UAVCAN #include AP_Baro_UAVCAN.h #endif #if HAL_WITH_UAVCAN if AP_BoardConfig::get_can_enable != 0 && hal.can_mgr != nullptr if _num_drivers < BARO_MAX_DRIVERS && _num_sensors < BARO_MAX_INSTANCES printf Creating AP_Baro_UAVCAN\n\r drivers _num_drivers = new AP_Baro_UAVCAN *this _num_drivers++ #endif"
ArduPilot 397 AP_Baro.cpp Control fd866d3e20 AP_Baro: removal of legacy UAVCAN support ""
ArduPilot 398 AP_Baro.cpp Non-Control 96f503d9d1 AP_Baro: Fix GND_EXT_BUS @Values formatting ""
ArduPilot 399 AP_Baro.cpp Non-Control cbea29ad71 AP_Baro: use sensor_config_error() " AP_BoardConfig::sensor_config_error Baro: unable to initialise driver "
ArduPilot 400 AP_Baro.cpp Non-Control ee4161fa62 AP_Baro: moved SITL baro to standard sensor backend model " #include AP_Baro_SITL.h #if CONFIG_HAL_BOARD == HAL_BOARD_SITL ADD_BACKEND new AP_Baro_SITL *this return #endif"
ArduPilot 401 AP_Baro.cpp Non-Control 9bdf971336 AP_Baro: added set_pressure_correction()for use by AP_TempCalibration " float pressure = sensors i .pressure + sensors i .p_correction altitude = get_altitude_difference sensors i .ground_pressure pressure void AP_Baro::set_pressure_correction uint8_t instance float p_correction if instance < _num_sensors sensors instance .p_correction = p_correction "
ArduPilot 402 AP_Baro.cpp Non-Control ed916327fc AP_Baro: Use SI units conventions in parameter unitsFollow the rules from:http://physics.nist.gov/cuu/Units/units.htmlhttp://physics.nist.gov/cuu/Units/outside.htmlandhttp://physics.nist.gov/cuu/Units/checklist.htmlone further constrain is that only printable (7bit) ASCII characters are allowed ""
ArduPilot 403 AP_Baro.cpp Control 6e6efa7e1b AP_Baro: enumeration and multiple interfaces support " #include #if HAL_WITH_UAVCAN bool added do added = _add_backend AP_Baro_UAVCAN::probe *this if _num_drivers == BARO_MAX_DRIVERS || _num_sensors == BARO_MAX_INSTANCES return while added #endif"
ArduPilot 404 AP_Baro.cpp Control 6982e58cf2 Baro: health check that values are changing " sensors i .healthy = now - sensors i .last_update_ms < BARO_TIMEOUT_MS && now - sensors i .last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS && !is_zero sensors i .pressure "
ArduPilot 406 AC_WPNav.cpp Control 39bc3800c9 AC_WPNav: add distance and bearing to target methods " float AC_WPNav::get_distance_to_target return _distance_to_target int32_t AC_WPNav::get_bearing_to_target return get_bearing_cd _inav->get_position _target void AC_WPNav::update_loiter uint32_t now = hal.scheduler->millis float dt = now - _last_update _last_update = now if dt >= 1.0 dt = 0.0 reset_I get_loiter_pos_lat_lon _target.x _target.y dt float AC_WPNav::get_distance_to_destination Vector3f curr = _inav->get_position return pythagorous2 _destination.x-curr.x _destination.y-curr.y int32_t AC_WPNav::get_bearing_to_destination return get_bearing_cd _inav->get_position _destination void AC_WPNav::update_wpnav uint32_t now = hal.scheduler->millis float dt = now - _last_update _last_update = now if dt >= 1.0 dt = 0.0 reset_I else _distance_to_target = linear_distance float AC_WPNav::get_bearing_cd const Vector3f origin const Vector3f destination float bearing = 9000 + atan2f - destination.x-origin.x destination.y-origin.y * 5729.57795f if bearing < 0 bearing += 36000 return bearing void AC_WPNav::reset_I _pid_pos_lon->reset_I _pid_pos_lat->reset_I _pid_rate_lon->reset_I _pid_rate_lat->reset_I "
ArduPilot 407 AC_WPNav.cpp Control 0d70ba1030 AC_WPNav: bug fix to dt calculation " AC_WPNav::AC_WPNav AP_InertialNav* inav APM_PI* pid_pos_lat APM_PI* pid_pos_lon AC_PID* pid_rate_lat AC_PID* pid_rate_lon : _pid_rate_lat pid_rate_lat _pid_rate_lon pid_rate_lon float dt = now - _last_update / 1000.0f float dt = now - _last_update / 1000.0f "
ArduPilot 408 AC_WPNav.cpp Non-Control 60060d8184 AC_WPNav: add get_target_altRequired because waypoint controller will be modified to controlaltitude " _target.z = _destination.z "
ArduPilot 409 AC_WPNav.cpp Control 8ec7fd1c9c AC_WPNav: add set_loiter_target to use velocityTarget can be set in front of the copter to reduce the bounce back tothe position where loiter was engaged. " void AC_WPNav::set_loiter_target const Vector3f& position const Vector3f& velocity float linear_distance float linear_velocity float vel_total float target_dist if _pid_pos_lat->kP <= 0.1 set_loiter_target position return linear_velocity = MAX_LOITER_POS_ACCEL/_pid_pos_lat->kP vel_total = safe_sqrt velocity.x*velocity.x + velocity.y*velocity.y if vel_total < linear_velocity target_dist = vel_total/_pid_pos_lat->kP else linear_distance = MAX_LOITER_POS_ACCEL/ 2*_pid_pos_lat->kP *_pid_pos_lat->kP target_dist = linear_distance + vel_total*vel_total / 2*MAX_LOITER_POS_ACCEL target_dist = constrain target_dist 0 MAX_LOITER_OVERSHOOT _target.x = position.x + target_dist * velocity.x / vel_total _target.y = position.y + target_dist * velocity.y / vel_total "
ArduPilot 410 AC_WPNav.cpp Control afd2f82768 AC_WPNav: bug fix for track covered being 2DReplaced abs and fabs with fabsfAdded get_destination method " _track_length = safe_sqrt _pos_delta.x * _pos_delta.x + _pos_delta.y * _pos_delta.y cross_track_dist = fabsf curr.x - _destination.x track_covered = fabsf curr.y - _origin.y cross_track_dist = fabsf curr.y - _destination.y track_covered = fabsf curr.x - _origin.x cross_track_dist = fabsf line_a * curr.x + line_b * curr.y + line_c / _track_length track_covered = fabsf line_a*_origin.x + line_b*_origin.y + line_c / safe_sqrt line_a*line_a+line_b*line_b "
ArduPilot 411 AC_WPNav.cpp Control f82ce449d7 AC_WPNav: add angle limits and set from AC s throttle controller " _pid_rate_lon pid_rate_lon _lean_angle_max MAX_LEAN_ANGLE _desired_roll = constrain accel_right/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max _desired_pitch = constrain -accel_forward/ -z_accel_meas*_cos_roll * 18000/M_PI -_lean_angle_max _lean_angle_max "
ArduPilot 412 AC_WPNav.cpp Control 926c404994 AC_WPNav: Leonard s loiter target smoothing " void AC_WPNav::move_loiter_target float vel_forward_cms float vel_right_cms float dt _pilot_vel_forward_cms = vel_forward_cms _pilot_vel_right_cms = vel_right_cms void AC_WPNav::translate_loiter_target_movements float nav_dt Vector2f target_vel_adj float vel_delta_total float vel_max float vel_total if nav_dt < 0 return target_vel_adj.x = _pilot_vel_forward_cms*_cos_yaw - _pilot_vel_right_cms*_sin_yaw - _target_vel.x target_vel_adj.y = _pilot_vel_forward_cms*_sin_yaw + _pilot_vel_right_cms*_cos_yaw - _target_vel.y vel_delta_total = safe_sqrt target_vel_adj.x*target_vel_adj.x + target_vel_adj.y*target_vel_adj.y vel_max = MAX_LOITER_POS_ACCEL*nav_dt if vel_delta_total > vel_max target_vel_adj.x = vel_max * target_vel_adj.x/vel_delta_total target_vel_adj.y = vel_max * target_vel_adj.y/vel_delta_total _target_vel.x += target_vel_adj.x _target_vel.y += target_vel_adj.y vel_total = safe_sqrt _target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y _target_vel.x = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.x/vel_total _target_vel.y = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.y/vel_total _target.x += _target_vel.x * nav_dt _target.y += _target_vel.y * nav_dt _target_vel.x = 0 _target_vel.y = 0 translate_loiter_target_movements dt "
ArduPilot 413 AC_WPNav.cpp Control 0ccdce1b22 AC_WPNav: Leonard s ff loiter and 3d wp nav " _speedz_cms MAX_CLIMB_VELOCITY _pos_delta_unit = _destination - _origin _track_length = _pos_delta_unit.length _pos_delta_unit = _pos_delta_unit/_track_length _hoz_track_ratio = _track_length / sqrt _pos_delta_unit.x*_pos_delta_unit.x + _pos_delta_unit.y*_pos_delta_unit.y _vert_track_ratio = _track_length / _pos_delta_unit.z float alt_error velocity_cms = min velocity_cms _speed_cms * _hoz_track_ratio velocity_cms = min velocity_cms _speedz_cms * _vert_track_ratio if _pos_delta_unit.x == 0 && _pos_delta_unit.y == 0 track_covered = curr.x-_origin.x * _pos_delta_unit.x + curr.y-_origin.y * _pos_delta_unit.y + curr.z-_origin.z * _pos_delta_unit.z cross_track_dist = - curr.x-_origin.x * _pos_delta_unit.y + curr.y-_origin.y * _pos_delta_unit.x alt_error = fabsf _origin.z + _pos_delta_unit.z * track_covered - curr.z track_desired_max = track_covered + min safe_sqrt WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - cross_track_dist*cross_track_dist * _hoz_track_ratio 750-alt_error * _vert_track_ratio _target.x = _origin.x + _pos_delta_unit.x * _track_desired _target.y = _origin.y + _pos_delta_unit.y * _track_desired _target.z = _origin.z + _pos_delta_unit.z * _track_desired void AC_WPNav::get_loiter_vel_lat_lon float vel_lat float vel_lon float dt Vector3f vel_curr = _inav->get_velocity Vector3f vel_error Vector2f desired_accel float accel_total if dt == 0.0 desired_accel.x = 0 desired_accel.y = 0 else desired_accel.x = vel_lat - _vel_last.x /dt desired_accel.y = vel_lon - _vel_last.y /dt _vel_last.x = vel_lat _vel_last.y = vel_lon vel_error.x = vel_lat - vel_curr.x vel_error.y = vel_lon - vel_curr.y desired_accel.x += _pid_rate_lat->get_pid vel_error.x dt desired_accel.y += _pid_rate_lon->get_pid vel_error.y dt accel_total = safe_sqrt desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y desired_accel.x = MAX_LOITER_VEL_ACCEL * desired_accel.x/accel_total desired_accel.y = MAX_LOITER_VEL_ACCEL * desired_accel.y/accel_total get_loiter_accel_lat_lon desired_accel.x desired_accel.y _vel_last = _inav->get_velocity "
ArduPilot 414 AC_WPNav.cpp Control 31838b2865 AC_WPNAV: change loiter controllers to use floatsin particular get_loiter_pos_lat_lon and get_loiter_accel_lat_lon " void AC_WPNav::get_loiter_pos_lat_lon float target_lat_from_home float target_lon_from_home float dt Vector2f dist_error Vector2f desired_vel float dist_error_total float vel_sqrt float vel_total float linear_distance dist_error.x = target_lat_from_home - _inav->get_latitude_diff dist_error.y = target_lon_from_home - _inav->get_longitude_diff dist_error_total = safe_sqrt dist_error.x*dist_error.x + dist_error.y*dist_error.y desired_vel.x = vel_sqrt * dist_error.x/dist_error_total desired_vel.y = vel_sqrt * dist_error.y/dist_error_total desired_vel.x = _pid_pos_lat->get_p dist_error.x desired_vel.y = _pid_pos_lon->get_p dist_error.y vel_total = safe_sqrt desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y desired_vel.x = MAX_LOITER_POS_VELOCITY * desired_vel.x/vel_total desired_vel.y = MAX_LOITER_POS_VELOCITY * desired_vel.y/vel_total get_loiter_vel_lat_lon desired_vel.x desired_vel.y dt void AC_WPNav::get_loiter_accel_lat_lon float accel_lat float accel_lon "
ArduPilot 415 AC_WPNav.cpp Control 6dbcbdcb43 AC_WPNav: limit max loiter position errormove interpretation of pilot input to wpnav lib " void AC_WPNav::move_loiter_target float control_roll float control_pitch float dt _pilot_vel_forward_cms = -control_pitch * MAX_LOITER_POS_VELOCITY / 4500.0f _pilot_vel_right_cms = control_roll * MAX_LOITER_POS_VELOCITY / 4500.0f Vector3f curr_pos = _inav->get_position Vector3f distance_err = _target - curr_pos float distance = safe_sqrt distance_err.x*distance_err.x + distance_err.y*distance_err.y if distance > MAX_LOITER_OVERSHOOT _target.x = curr_pos.x + MAX_LOITER_OVERSHOOT * distance_err.x/distance _target.y = curr_pos.y + MAX_LOITER_OVERSHOOT * distance_err.y/distance "
ArduPilot 416 AC_WPNav.cpp Control 74e1c2e660 AC_WPNav: advance track fixes " Vector3f pos_delta = destination - origin _track_length = pos_delta.length _pos_delta_pct = pos_delta/_track_length if pos_delta.x == 0 && pos_delta.y == 0 _hoz_track_ratio = 0 else _hoz_track_ratio = _track_length / safe_sqrt pos_delta.x*pos_delta.x + pos_delta.y*pos_delta.y if pos_delta.z == 0 _vert_track_ratio = 0 else _vert_track_ratio = _track_length / fabsf pos_delta.z float hor_track_allowance vert_track_allowance if _hoz_track_ratio >= 0 velocity_cms = min velocity_cms _speed_cms * _hoz_track_ratio if _vert_track_ratio >= 0 velocity_cms = min velocity_cms _speedz_cms * _vert_track_ratio track_covered = curr.x-_origin.x * _pos_delta_pct.x + curr.y-_origin.y * _pos_delta_pct.y + curr.z-_origin.z * _pos_delta_pct.z cross_track_dist = - curr.x-_origin.x * _pos_delta_pct.y + curr.y-_origin.y * _pos_delta_pct.x alt_error = fabsf _origin.z + _pos_delta_pct.z * track_covered - curr.z if _hoz_track_ratio > 0 hor_track_allowance = safe_sqrt WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - cross_track_dist*cross_track_dist * _hoz_track_ratio hor_track_allowance = max hor_track_allowance 0 if _vert_track_ratio > 0 vert_track_allowance = 750-alt_error * _vert_track_ratio vert_track_allowance = max vert_track_allowance 0 if _hoz_track_ratio > 0 && _vert_track_ratio track_desired_max = track_covered + min hor_track_allowance vert_track_allowance else if _hoz_track_ratio > 0 track_desired_max = track_covered + hor_track_allowance else track_desired_max = track_covered + vert_track_allowance _track_desired = constrain _track_desired 0 _track_length _target.x = _origin.x + _pos_delta_pct.x * _track_desired _target.y = _origin.y + _pos_delta_pct.y * _track_desired _target.z = _origin.z + _pos_delta_pct.z * _track_desired "
ArduPilot 417 AC_WPNav.cpp Control c6b68c7843 AC_WPNav: Leonard s 3d leash calculator " if vel_total > MAX_LOITER_POS_VELOCITY _target_vel.x = MAX_LOITER_POS_VELOCITY * _target_vel.x/vel_total _target_vel.y = MAX_LOITER_POS_VELOCITY * _target_vel.y/vel_total _vert_track_scale = WPINAV_MAX_POS_ERROR / WPINAV_MAX_ALT_ERROR Vector3f pos_delta = _destination - _origin pos_delta.z = pos_delta.z * _vert_track_scale _track_length = pos_delta.length _pos_delta_unit = pos_delta/_track_length float track_error float track_extra_max float curr_delta_length Vector3f curr_delta = _inav->get_position - _origin curr_delta.z = curr_delta.z * _vert_track_scale curr_delta_length = curr_delta.length track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z track_error = safe_sqrt curr_delta_length*curr_delta_length - track_covered*track_covered track_extra_max = safe_sqrt WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error track_desired_max = track_covered + track_extra_max _target.x = _origin.x + _pos_delta_unit.x * _track_desired _target.y = _origin.y + _pos_delta_unit.y * _track_desired _target.z = _origin.z + _pos_delta_unit.z * _track_desired /_vert_track_scale "
ArduPilot 418 AC_WPNav.cpp Non-Control 35001619f0 AC_WPNav: stop track_desired from moving backwards " float track_desired_temp = _track_desired track_desired_temp += velocity_cms * dt if track_desired_temp > track_desired_max track_desired_temp = track_desired_max track_desired_temp = constrain track_desired_temp 0 _track_length _track_desired = max _track_desired track_desired_temp "
ArduPilot 419 AC_WPNav.cpp Control 9d7d174995 AC_WPNAV: check distance to waypoint within librarybug fix for loiter using lat/lon position instead of NED position whencalculating desired velocity towards target " AP_GROUPINFO SPEED 0 AC_WPNav _speed_cms WPNAV_WP_SPEED AP_GROUPINFO RADIUS 1 AC_WPNav _wp_radius_cm WPNAV_WP_RADIUS _reached_destination = false Vector3f curr_pos = _inav->get_position Vector3f curr_delta = curr_pos - _origin if !_reached_destination if _track_desired >= _track_length Vector3f dist_to_dest = curr_pos - _destination dist_to_dest.z *=_vert_track_scale if dist_to_dest.length <= _wp_radius_cm _reached_destination = true Vector3f curr = _inav->get_position dist_error.x = target_lat_from_home - curr.x dist_error.y = target_lon_from_home - curr.y "
ArduPilot 420 AC_WPNav.cpp Non-Control 252e98ec57 AC_WPNav: use global gravity constant " float z_accel_meas = -GRAVITY_MSS * 100 "
ArduPilot 421 AC_WPNav.cpp Non-Control 0351c2ae33 AC_WPNav: allow setting of horizontal velocityAlso added accessor function for waypoint radius parameter " AP_GROUPINFO SPEED 0 AC_WPNav _speed_cms WPNAV_WP_SPEED AP_GROUPINFO RADIUS 1 AC_WPNav _wp_radius_cm WPNAV_WP_RADIUS "
ArduPilot 422 AC_WPNav.cpp Control 0eab3e87b2 AC_WPNav: use prev wp as origin for next wp " Vector3f AC_WPNav::project_stopping_point const Vector3f& position const Vector3f& velocity Vector3f target return position target.x = position.x + target_dist * velocity.x / vel_total target.y = position.y + target_dist * velocity.y / vel_total target.z = position.z return target void AC_WPNav::set_loiter_target const Vector3f& position const Vector3f& velocity Vector3f target = project_stopping_point position velocity _target.x = target.x _target.y = target.y float dt = now - _loiter_last_update / 1000.0f _loiter_last_update = now Vector3f origin if _reached_destination && hal.scheduler->millis - _wpnav_last_update < 1000 origin = _destination else origin = project_stopping_point _inav->get_position _inav->get_velocity set_origin_and_destination origin destination float dt = now - _wpnav_last_update / 1000.0f _wpnav_last_update = now _target_vel.x = 0 _target_vel.y = 0 "
ArduPilot 423 AC_WPNav.cpp Non-Control 8fe3e689f4 Copter: add WPNAV log message " get_loiter_position_to_velocity dt get_loiter_position_to_velocity dt void AC_WPNav::get_loiter_position_to_velocity float dt dist_error.x = _target.x - curr.x dist_error.y = _target.y - curr.y get_loiter_velocity_to_acceleration desired_vel.x desired_vel.y dt void AC_WPNav::get_loiter_velocity_to_acceleration float vel_lat float vel_lon float dt get_loiter_acceleration_to_lean_angles desired_accel.x desired_accel.y void AC_WPNav::get_loiter_acceleration_to_lean_angles float accel_lat float accel_lon "
ArduPilot 424 AC_WPNav.cpp Control effd5b0da7 AC_WPNav: improved commentsMade SPEEDZ a parameter although it s not yet used " AP_GROUPINFO RADIUS 1 AC_WPNav _wp_radius_cm WPNAV_WP_RADIUS AP_GROUPINFO SPEEDZ 2 AC_WPNav _speedz_cms WPNAV_WP_SPEEDZ Vector2f target_vel_adj _vert_track_scale = WPNAV_LEASH_XY / WPNAV_LEASH_Z track_extra_max = safe_sqrt WPNAV_LEASH_XY*WPNAV_LEASH_XY - track_error*track_error "
ArduPilot 425 AC_WPNav.cpp Control 764853bd56 AC_WPNav: add climb and descent params dynamic leash length " AP_GROUPINFO SPEED 0 AC_WPNav _speed_xy_cms WPNAV_WP_SPEED AP_GROUPINFO SPEED_UP 2 AC_WPNav _speed_up_cms WPNAV_WP_SPEED_UP AP_GROUPINFO SPEED_DN 3 AC_WPNav _speed_down_cms WPNAV_WP_SPEED_DOWN bool climb = pos_delta.z >= 0 calculate_leash_length climb track_extra_max = safe_sqrt _leash_xy*_leash_xy - track_error*track_error advance_target_along_track _speed_xy_cms dt void AC_WPNav::calculate_leash_length bool climb float kP = _pid_pos_lat->kP if kP <= 0.51 kP = 0.51 if _speed_xy_cms <= MAX_LOITER_POS_ACCEL / kP _leash_xy = _speed_xy_cms / kP else _leash_xy = MAX_LOITER_POS_ACCEL / 2.0*kP*kP + _speed_xy_cms*_speed_xy_cms / 2*MAX_LOITER_POS_ACCEL if _leash_xy < 100 _leash_xy = 100 float speed_vert leash_z if climb speed_vert = _speed_up_cms else speed_vert = _speed_down_cms if speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / WPNAV_ALT_HOLD_P leash_z = speed_vert / WPNAV_ALT_HOLD_P else leash_z = WPNAV_ALT_HOLD_ACCEL_MAX / 2.0*WPNAV_ALT_HOLD_P*WPNAV_ALT_HOLD_P + speed_vert*speed_vert / 2*WPNAV_ALT_HOLD_ACCEL_MAX if leash_z < 100 leash_z = 100 _vert_track_scale = _leash_xy / leash_z "
ArduPilot 426 AC_WPNav.cpp Non-Control ed298363f4 AC_WPNav: remove unnecessary limit in leash calc ""
ArduPilot 427 AC_WPNav.cpp Control 2fc25da4c3 Copter: Leonard s fix for get_loiter_accel_to_lean_angle " _desired_roll = constrain accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max _desired_pitch = constrain -accel_forward/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max "
ArduPilot 428 AC_WPNav.cpp Control ad40ba57be AC_WPNav: optimise to reduce memory copiessave a few cyclesPair-Programmed-With: Randy Mackay " void AC_WPNav::project_stopping_point const Vector3f& position const Vector3f& velocity Vector3f &target target = position return Vector3f target project_stopping_point position velocity target float AC_WPNav::get_distance_to_target const int32_t AC_WPNav::get_bearing_to_target const _origin = _destination project_stopping_point _inav->get_position _inav->get_velocity _origin set_origin_and_destination _origin destination float AC_WPNav::get_bearing_cd const Vector3f &origin const Vector3f &destination const "
ArduPilot 429 AC_WPNav.cpp Control 6c081c3def AC_WPNav: slow acceleration of intermediate point " void AC_WPNav::advance_target_along_track float dt if dt > 0 && _limited_speed_xy_cms < _speed_xy_cms _limited_speed_xy_cms += WPNAV_WP_ACCELERATION * dt if _limited_speed_xy_cms > _speed_xy_cms _limited_speed_xy_cms = _speed_xy_cms track_desired_temp += _limited_speed_xy_cms * dt advance_target_along_track dt _limited_speed_xy_cms = 0 "
ArduPilot 430 AC_WPNav.cpp Non-Control ba83950fc4 libraries: replace constrain() with constrain_float()this makes the type much more obvious. Thanks to Tobias for thesuggestion. " target_dist = constrain_float target_dist 0 MAX_LOITER_OVERSHOOT track_desired_temp = constrain_float track_desired_temp 0 _track_length vel_sqrt = constrain_float safe_sqrt 2*MAX_LOITER_POS_ACCEL* dist_error_total-linear_distance 0 1000 _desired_roll = constrain_float accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max _desired_pitch = constrain_float -accel_forward/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max "
ArduPilot 431 AC_WPNav.cpp Control 647a93e78e Copter: remove jerk when entering RTL or AUTOLoiter target position was not being initialised properly.Add a few more comments " _loiter_last_update 0 _wpnav_last_update 0 _cos_yaw 1.0 _sin_yaw 0.0 _cos_pitch 1.0 _desired_roll 0 _desired_pitch 0 _target 0 0 0 _pilot_vel_forward_cms 0 _pilot_vel_right_cms 0 _target_vel 0 0 0 _vel_last 0 0 0 _lean_angle_max MAX_LEAN_ANGLE dist_error 0 0 desired_vel 0 0 desired_accel 0 0 bool climb = pos_delta.z >= 0 _target = origin "
ArduPilot 432 AC_WPNav.cpp Non-Control 1f8161b493 AC_WPNav: remove unnecessary speed constraint " vel_sqrt = safe_sqrt 2*MAX_LOITER_POS_ACCEL* dist_error_total-linear_distance "
ArduPilot 433 AC_WPNav.cpp Control 328d900647 WPNav: flexible loiter speedAlso includes bug fix to allow speeds > 10m/s during missions " AP_GROUPINFO SPEED 0 AC_WPNav _wp_speed_cms WPNAV_WP_SPEED AP_GROUPINFO RADIUS 1 AC_WPNav _wp_radius_cm WPNAV_WP_RADIUS AP_GROUPINFO SPEED_UP 2 AC_WPNav _wp_speed_up_cms WPNAV_WP_SPEED_UP AP_GROUPINFO SPEED_DN 3 AC_WPNav _wp_speed_down_cms WPNAV_WP_SPEED_DOWN AP_GROUPINFO LOIT_SPEED 4 AC_WPNav _loiter_speed_cms WPNAV_LOITER_SPEED calculate_loiter_leash_length target_dist = constrain_float target_dist 0 _loiter_leash calculate_loiter_leash_length _pilot_vel_forward_cms = -control_pitch * _loiter_speed_cms / 4500.0f _pilot_vel_right_cms = control_roll * _loiter_speed_cms / 4500.0f if vel_total > _loiter_speed_cms _target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total _target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total if distance > _loiter_leash _target.x = curr_pos.x + _loiter_leash * distance_err.x/distance _target.y = curr_pos.y + _loiter_leash * distance_err.y/distance get_loiter_position_to_velocity dt _loiter_speed_cms void AC_WPNav::calculate_loiter_leash_length float kP = _pid_pos_lat->kP if _loiter_speed_cms <= MAX_LOITER_POS_ACCEL / kP _loiter_leash = _loiter_speed_cms / kP else _loiter_leash = MAX_LOITER_POS_ACCEL / 2.0*kP*kP + _loiter_speed_cms*_loiter_speed_cms / 2*MAX_LOITER_POS_ACCEL if _loiter_leash < 100 _loiter_leash = 100 calculate_wp_leash_length climb if dt > 0 && _limited_speed_xy_cms < _wp_speed_cms if _limited_speed_xy_cms > _wp_speed_cms _limited_speed_xy_cms = _wp_speed_cms track_extra_max = safe_sqrt _wp_leash_xy*_wp_leash_xy - track_error*track_error get_loiter_position_to_velocity dt _wp_speed_cms void AC_WPNav::get_loiter_position_to_velocity float dt float max_speed_cms if vel_total > max_speed_cms desired_vel.x = max_speed_cms * desired_vel.x/vel_total desired_vel.y = max_speed_cms * desired_vel.y/vel_total void AC_WPNav::calculate_wp_leash_length bool climb if _wp_speed_cms <= MAX_LOITER_POS_ACCEL / kP _wp_leash_xy = _wp_speed_cms / kP _wp_leash_xy = MAX_LOITER_POS_ACCEL / 2.0*kP*kP + _wp_speed_cms*_wp_speed_cms / 2*MAX_LOITER_POS_ACCEL if _wp_leash_xy < 100 _wp_leash_xy = 100 speed_vert = _wp_speed_up_cms speed_vert = _wp_speed_down_cms _vert_track_scale = _wp_leash_xy / leash_z "
ArduPilot 434 AC_WPNav.cpp Control 4704b729c2 WPNav: reduce max acceleration to 2.5m/sAlso includes bug fix to ramp up of speed of intermediate point " _limited_speed_xy_cms = 0 "
ArduPilot 435 AC_WPNav.cpp Control 9f735c8d03 WPNav: add fast waypoints flagWaypoint is considered reached when the intermediate point reaches thedestination " if _flags.reached_destination && hal.scheduler->millis - _wpnav_last_update < 1000 _flags.reached_destination = false _flags.fast_waypoint = false if !_flags.reached_destination if _flags.fast_waypoint _flags.reached_destination = true else Vector3f dist_to_dest = curr_pos - _destination dist_to_dest.z *=_vert_track_scale if dist_to_dest.length <= _wp_radius_cm _flags.reached_destination = true "
ArduPilot 436 AC_WPNav.cpp Control ed5ddfd9db WPNav: initialise desired roll pitchbug-fix to resolve 1/10th second twitch when loiter or waypointcontroller are first engaged " AC_WPNav::AC_WPNav AP_InertialNav* inav AP_AHRS* ahrs APM_PI* pid_pos_lat APM_PI* pid_pos_lon AC_PID* pid_rate_lat AC_PID* pid_rate_lon : _ahrs ahrs _desired_roll = constrain_int32 _ahrs->roll_sensor -MAX_LEAN_ANGLE MAX_LEAN_ANGLE _desired_pitch = constrain_int32 _ahrs->pitch_sensor -MAX_LEAN_ANGLE MAX_LEAN_ANGLE _desired_roll = constrain_int32 _ahrs->roll_sensor -MAX_LEAN_ANGLE MAX_LEAN_ANGLE _desired_pitch = constrain_int32 _ahrs->pitch_sensor -MAX_LEAN_ANGLE MAX_LEAN_ANGLE "
ArduPilot 437 AC_WPNav.cpp Non-Control 61288fcb90 WPNav: make get_stopping_point method public " void AC_WPNav::get_stopping_point const Vector3f& position const Vector3f& velocity Vector3f &target const get_stopping_point position velocity target get_stopping_point _inav->get_position _inav->get_velocity _origin "
ArduPilot 438 AC_WPNav.cpp Control 55235630b6 Copter: bug fix for slow loiter repositioning " vel_max = 2.0*MAX_LOITER_POS_ACCEL*nav_dt "
ArduPilot 439 AC_WPNav.cpp Control cb795ea536 WPNav: divide by zero checks " _loiter_leash WPNAV_MIN_LEASH_LENGTH _wp_leash_xy WPNAV_MIN_LEASH_LENGTH float kP = _pid_pos_lat->kP vel_total = safe_sqrt velocity.x*velocity.x + velocity.y*velocity.y if vel_total < 10.0f || kP <= 0.0f linear_velocity = MAX_LOITER_POS_ACCEL/kP target_dist = vel_total/kP linear_distance = MAX_LOITER_POS_ACCEL/ 2*kP*kP if vel_delta_total > vel_max && vel_delta_total > 0.0f if vel_total > _loiter_speed_cms && vel_total > 0.0f if distance > _loiter_leash && distance > 0.0f if kP <= 0.0f _loiter_leash = WPNAV_MIN_LEASH_LENGTH return if _loiter_leash < WPNAV_MIN_LEASH_LENGTH _loiter_leash = WPNAV_MIN_LEASH_LENGTH if _track_length == 0.0f _pos_delta_unit.x = 0 _pos_delta_unit.y = 0 _pos_delta_unit.z = 0 else _pos_delta_unit = pos_delta/_track_length float kP = _pid_pos_lat->kP if kP <= 0.0f desired_vel.x = 0.0 desired_vel.y = 0.0 dist_error.x = _target.x - curr.x dist_error.y = _target.y - curr.y linear_distance = MAX_LOITER_POS_ACCEL/ 2*kP*kP _distance_to_target = linear_distance dist_error_total = safe_sqrt dist_error.x*dist_error.x + dist_error.y*dist_error.y if dist_error_total > 2*linear_distance vel_sqrt = safe_sqrt 2*MAX_LOITER_POS_ACCEL* dist_error_total-linear_distance desired_vel.x = vel_sqrt * dist_error.x/dist_error_total desired_vel.y = vel_sqrt * dist_error.y/dist_error_total else desired_vel.x = _pid_pos_lat->get_p dist_error.x desired_vel.y = _pid_pos_lon->get_p dist_error.y vel_total = safe_sqrt desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y if vel_total > max_speed_cms desired_vel.x = max_speed_cms * desired_vel.x/vel_total desired_vel.y = max_speed_cms * desired_vel.y/vel_total if kP <= 0.0f _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH _vert_track_scale = 1.0f return if _wp_leash_xy < WPNAV_MIN_LEASH_LENGTH _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH if leash_z < WPNAV_MIN_LEASH_LENGTH leash_z = WPNAV_MIN_LEASH_LENGTH "
ArduPilot 440 AC_WPNav.cpp Control 400c1bd7b7 Copter: smooth waypoint transitionsSpeed of intermediate point is initialised and constrained based oncurrent speed vector vs direction to the next waypoint.This means the copter does not slow if the previous segment and nextsegment are in line and also the intermediate point is not advancedtowards the next waypoint if the copter is moving quickly in theopposite direction. " Vector3f curr_vel = _inav->get_velocity float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z _limited_speed_xy_cms = constrain_float speed_along_track 0 _wp_speed_cms Vector3f curr_vel = _inav->get_velocity float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z float linear_velocity = _wp_speed_cms float kP = _pid_pos_lat->kP if kP >= 0.0f linear_velocity = MAX_LOITER_POS_ACCEL/kP if speed_along_track < -linear_velocity _limited_speed_xy_cms = 0 else if dt > 0 && _limited_speed_xy_cms < _wp_speed_cms _limited_speed_xy_cms += WPNAV_WP_ACCELERATION * dt if _limited_speed_xy_cms > _wp_speed_cms _limited_speed_xy_cms = _wp_speed_cms if fabsf speed_along_track < linear_velocity _limited_speed_xy_cms = constrain_float _limited_speed_xy_cms speed_along_track-linear_velocity speed_along_track+linear_velocity "
ArduPilot 441 AC_WPNav.cpp Control 46d65150af Copter: lengthen loiter stopping pointThis will make stops less aggressive when entering loiter at very highspeeds (more 7.5m/s) " target_dist = constrain_float target_dist 0 _loiter_leash*2.0 "
ArduPilot 442 AC_WPNav.cpp Control 916f241fff Copter: Leonard s improved Loiter " linear_velocity = WPNAV_ACCELERATION/kP linear_distance = WPNAV_ACCELERATION/ 2*kP*kP target_dist = linear_distance + vel_total*vel_total / 2*WPNAV_ACCELERATION _target.x = position.x _target.y = position.y _target_vel.x = velocity.x _target_vel.y = velocity.y _pilot_vel_forward_cms = -control_pitch * WPNAV_LOITER_ACCEL_MAX / 4500.0f _pilot_vel_right_cms = control_roll * WPNAV_LOITER_ACCEL_MAX / 4500.0f if _loiter_speed_cms < 100.0f _loiter_speed_cms = 100.0f target_vel_adj.x = _pilot_vel_forward_cms*_cos_yaw - _pilot_vel_right_cms*_sin_yaw target_vel_adj.y = _pilot_vel_forward_cms*_sin_yaw + _pilot_vel_right_cms*_cos_yaw _target_vel.x += target_vel_adj.x*nav_dt _target_vel.y += target_vel_adj.y*nav_dt if _target_vel.x > 0 _target_vel.x -= WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.x/_loiter_speed_cms _target_vel.x = max _target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if _target_vel.x < 0 _target_vel.x -= WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.x/_loiter_speed_cms _target_vel.x = min _target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 if _target_vel.y > 0 _target_vel.y -= WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.y/_loiter_speed_cms _target_vel.y = max _target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if _target_vel.y < 0 _target_vel.y -= WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.y/_loiter_speed_cms _target_vel.y = min _target_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 if _loiter_speed_cms <= WPNAV_ACCELERATION / kP _loiter_leash = WPNAV_ACCELERATION / 2.0*kP*kP + _loiter_speed_cms*_loiter_speed_cms / 2*WPNAV_ACCELERATION else _target_vel.x = 0 _target_vel.y = 0 linear_velocity = WPNAV_ACCELERATION/kP linear_distance = WPNAV_ACCELERATION/ 2*kP*kP vel_sqrt = safe_sqrt 2*WPNAV_ACCELERATION* dist_error_total-linear_distance desired_vel.x += _target_vel.x desired_vel.y += _target_vel.y if accel_total > WPNAV_ACCEL_MAX desired_accel.x = WPNAV_ACCEL_MAX * desired_accel.x/accel_total desired_accel.y = WPNAV_ACCEL_MAX * desired_accel.y/accel_total if _wp_speed_cms <= WPNAV_ACCELERATION / kP _wp_leash_xy = WPNAV_ACCELERATION / 2.0*kP*kP + _wp_speed_cms*_wp_speed_cms / 2*WPNAV_ACCELERATION "
ArduPilot 443 AC_WPNav.cpp Non-Control a474b97291 Copter: resolve compiler warning re unused vars ""
ArduPilot 444 AC_WPNav.cpp Control e3dffb920b Copter: initialise target vel in loiter " void AC_WPNav::set_loiter_target const Vector3f& position _target = position _target_vel.x = 0 _target_vel.y = 0 "
ArduPilot 445 AC_WPNav.cpp Non-Control dbd6524f9f Copter: WPNav consolidate acceleration #defines " _limited_speed_xy_cms += 2.0f * WPNAV_ACCELERATION * dt "
ArduPilot 446 AC_WPNav.cpp Control d203f0295b Copter: bug fix for vertical speed during missions " _vert_speed_scale 1.0 _track_speed_scaler 1.0 calculate_wp_leash_length climb pos_delta.z = pos_delta.z * _track_vert_scale float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z*_vert_speed_scale * _pos_delta_unit.z _track_speed_scaler = safe_sqrt _pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y + _vert_speed_scale*fabsf _pos_delta_unit.z curr_delta.z = curr_delta.z * _track_vert_scale float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z*_vert_speed_scale * _pos_delta_unit.z track_desired_temp += _limited_speed_xy_cms * _track_speed_scaler * dt _target.z = _origin.z + _pos_delta_unit.z * _track_desired /_track_vert_scale dist_to_dest.z *= _track_vert_scale _track_vert_scale = 1.0f _track_vert_scale = _wp_leash_xy / leash_z if speed_vert <= 0 _vert_speed_scale = 1.0 else _vert_speed_scale = _wp_speed_cms / speed_vert "
ArduPilot 447 AC_WPNav.cpp Control a1926441da Copter: use fast tan for accel to lean angle calcsCommitted on behalf of Leonard " _desired_roll = constrain_float fast_atan accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max _desired_pitch = constrain_float fast_atan -accel_forward/ -z_accel_meas * 18000/M_PI -_lean_angle_max _lean_angle_max "
ArduPilot 448 AC_WPNav.cpp Control 8c4a7ec094 Copter WPNav: Leonard s improved speed fixAlso pass in althold gain from main code " _althold_kP WPNAV_ALT_HOLD_P _wp_leash_z WPNAV_MIN_LEASH_LENGTH _track_accel 0 _track_speed 0 _track_leash_length 0 _track_length = pos_delta.length calculate_wp_leash_length climb float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z Vector3f track_error track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z Vector3f track_covered_pos = _pos_delta_unit * track_covered track_error = curr_delta - track_covered_pos float track_error_xy = safe_sqrt track_error.x*track_error.x + track_error.y*track_error.y float track_error_z = fabsf track_error.z track_extra_max = min _track_leash_length* _wp_leash_z-track_error_z /_wp_leash_z _track_leash_length* _wp_leash_xy-track_error_xy /_wp_leash_xy if track_extra_max <0 track_desired_max = track_covered else track_desired_max = track_covered + track_extra_max float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z linear_velocity = _track_accel/kP if dt > 0 if track_desired_max > _track_desired _limited_speed_xy_cms += 2.0 * _track_accel * dt else _track_desired = track_desired_max if _limited_speed_xy_cms > _track_speed _limited_speed_xy_cms = _track_speed track_desired_temp += _limited_speed_xy_cms * dt _target = _origin + _pos_delta_unit * _track_desired float speed_vert if speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / _althold_kP _wp_leash_z = speed_vert / _althold_kP _wp_leash_z = WPNAV_ALT_HOLD_ACCEL_MAX / 2.0*_althold_kP*_althold_kP + speed_vert*speed_vert / 2*WPNAV_ALT_HOLD_ACCEL_MAX if _wp_leash_z < WPNAV_MIN_LEASH_LENGTH _wp_leash_z = WPNAV_MIN_LEASH_LENGTH float pos_delta_unit_xy = sqrt _pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y float pos_delta_unit_z = fabsf _pos_delta_unit.z if pos_delta_unit_z == 0 && pos_delta_unit_xy == 0 _track_accel = 0 _track_speed = 0 _track_leash_length = WPNAV_MIN_LEASH_LENGTH else if _pos_delta_unit.z == 0 _track_accel = WPNAV_ACCELERATION/pos_delta_unit_xy _track_speed = _wp_speed_cms/pos_delta_unit_xy _track_leash_length = _wp_leash_xy/pos_delta_unit_xy else if pos_delta_unit_xy == 0 _track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z _track_speed = speed_vert/pos_delta_unit_z _track_leash_length = _wp_leash_z/pos_delta_unit_z else _track_accel = min WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z WPNAV_ACCELERATION/pos_delta_unit_xy _track_speed = min speed_vert/pos_delta_unit_z _wp_speed_cms/pos_delta_unit_xy _track_leash_length = min _wp_leash_z/pos_delta_unit_z _wp_leash_xy/pos_delta_unit_xy "
ArduPilot 449 AC_WPNav.cpp Non-Control 89bbf5844f Copter WPNav: remove unused variable ""
ArduPilot 450 AC_WPNav.cpp Control 272f0e5032 Copter: reduce twitch when entering CIRCLE modeSet loiter target and prev iterations velocity when circle mode isstartedStart circling from projected stopping point " void AC_WPNav::init_loiter_target const Vector3f& position const Vector3f& velocity _pilot_vel_forward_cms = 0 _pilot_vel_right_cms = 0 _vel_last = _inav->get_velocity "
ArduPilot 451 AC_WPNav.cpp Control dde19c9585 WPNav: add acceleration parameterWP_ACCEL added to allow user control of acceleration during missions.Loiter acceleration made to be half of loiter max speed " AP_GROUPINFO ACCEL 5 AC_WPNav _wp_accel_cms WPNAV_ACCELERATION _loiter_accel_cms WPNAV_LOITER_ACCEL_MAX if vel_total < 10.0f || kP <= 0.0f || _wp_accel_cms <= 0.0f linear_velocity = _wp_accel_cms/kP linear_distance = _wp_accel_cms/ 2.0f*kP*kP target_dist = linear_distance + vel_total*vel_total / 2.0f*_wp_accel_cms target_dist = constrain_float target_dist 0 _loiter_leash*2.0f if kP <= 0.0f || _wp_accel_cms <= 0.0f _loiter_accel_cms = _loiter_leash / 2.0f if _loiter_speed_cms <= _wp_accel_cms / kP _loiter_leash = _wp_accel_cms / 2.0f*kP*kP + _loiter_speed_cms*_loiter_speed_cms / 2.0f*_wp_accel_cms _loiter_accel_cms = _loiter_leash / 2.0f linear_distance = _wp_accel_cms/ 2.0f*kP*kP if dist_error_total > 2.0f*linear_distance vel_sqrt = safe_sqrt 2.0f*_wp_accel_cms* dist_error_total-linear_distance if _wp_accel_cms <= 0.0f _wp_accel_cms = WPNAV_ACCELERATION_MIN if _wp_speed_cms <= _wp_accel_cms / kP _wp_leash_xy = _wp_accel_cms / 2.0f*kP*kP + _wp_speed_cms*_wp_speed_cms / 2.0f*_wp_accel_cms _track_accel = _wp_accel_cms/pos_delta_unit_xy _track_accel = min WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z _wp_accel_cms/pos_delta_unit_xy "
ArduPilot 452 AC_WPNav.cpp Control 5d23d5aaa7 WPNav: reduce loiter speed used to correct pos errorContributed by Leonard HallThis should reduce the aggressiveness of the response when we experiencea GPS glitch " get_loiter_position_to_velocity dt WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR "
ArduPilot 453 AC_WPNav.cpp Control c4f17b3235 WPNav: rounding error fix in loiterContributed by Leonard Hall " desired_vel.x = _pid_pos_lat->kP * dist_error.x desired_vel.y = _pid_pos_lon->kP * dist_error.y "
ArduPilot 454 AC_WPNav.cpp Control 518eba0729 WPNav: bug fix to loiter accel calculationContribution from Leonard Hall " if _loiter_speed_cms < 100.0f _loiter_speed_cms = 100.0f _loiter_accel_cms = _loiter_speed_cms / 2.0f if WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR <= _wp_accel_cms / kP _loiter_leash = WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / kP _loiter_leash = _wp_accel_cms / 2.0f*kP*kP + WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR*WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / 2.0f*_wp_accel_cms "
ArduPilot 455 AC_WPNav.cpp Non-Control 204f613467 WPNav: bug fix to reported distance to targetThis value is for reporting purposes only " _distance_to_target = dist_error_total "
ArduPilot 456 AC_WPNav.cpp Non-Control 345924ddec WPNav: replace LOITER_ACCEL_MAX with parameterAlso removed unused _lean_angle_max variable " _pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f _pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f _target_vel.x -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.x/_loiter_speed_cms _target_vel.x -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.x/_loiter_speed_cms _target_vel.y -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.y/_loiter_speed_cms _target_vel.y -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*_target_vel.y/_loiter_speed_cms _desired_roll = constrain_float fast_atan accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI -MAX_LEAN_ANGLE MAX_LEAN_ANGLE _desired_pitch = constrain_float fast_atan -accel_forward/ -z_accel_meas * 18000/M_PI -MAX_LEAN_ANGLE MAX_LEAN_ANGLE "
ArduPilot 457 AC_WPNav.cpp Non-Control bf5a50f738 WPNav: stopping point projection uses wp_leash " target_dist = constrain_float target_dist 0 _wp_leash_xy*2.0f "
ArduPilot 458 AC_WPNav.cpp Control 6969ab573d Copter: configurable max lean angleANGLE_MAX parameter allows limiting the roll and pitch angles duringmanual and auto flight modes to anywhere from 10 to 80 degrees " _lean_angle_max_cd MAX_LEAN_ANGLE _desired_roll = constrain_int32 _ahrs->roll_sensor -_lean_angle_max_cd _lean_angle_max_cd _desired_pitch = constrain_int32 _ahrs->pitch_sensor -_lean_angle_max_cd _lean_angle_max_cd _desired_roll = constrain_int32 _ahrs->roll_sensor -_lean_angle_max_cd _lean_angle_max_cd _desired_pitch = constrain_int32 _ahrs->pitch_sensor -_lean_angle_max_cd _lean_angle_max_cd _desired_roll = constrain_float fast_atan accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI -_lean_angle_max_cd _lean_angle_max_cd _desired_pitch = constrain_float fast_atan -accel_forward/ -z_accel_meas * 18000/M_PI -_lean_angle_max_cd _lean_angle_max_cd "
ArduPilot 459 AC_WPNav.cpp Control 45aeb1a921 Copter: split up loiter into 4 stepsReduces disruption to the main loop but also introduces a delay of 30msto navigation output " _loiter_step = 0 if dt > 0.095f && _loiter_step > 3 _loiter_step = 0 switch _loiter_step case 0: _loiter_dt = dt _loiter_last_update = now translate_loiter_target_movements _loiter_dt _loiter_step++ break case 1: get_loiter_position_to_velocity _loiter_dt WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR _loiter_step++ break case 2: get_loiter_velocity_to_acceleration desired_vel.x desired_vel.y _loiter_dt _loiter_step++ break case 3: get_loiter_acceleration_to_lean_angles desired_accel.x desired_accel.y _loiter_step++ break _wpnav_step = 0 if dt > 0.095f && _wpnav_step > 3 _wpnav_step = 0 switch _wpnav_step case 0: _wpnav_dt = dt _wpnav_last_update = now if dt > 0.0f advance_target_along_track dt _wpnav_step++ break case 1: get_loiter_position_to_velocity _wpnav_dt _wp_speed_cms _wpnav_step++ break case 2: get_loiter_velocity_to_acceleration desired_vel.x desired_vel.y _wpnav_dt _wpnav_step++ break case 3: get_loiter_acceleration_to_lean_angles desired_accel.x desired_accel.y _wpnav_step++ break "
ArduPilot 460 AC_WPNav.cpp Non-Control 1ecb583dd9 AC_WPNav: replace unnecessary objects with const refs " const Vector3f &curr_vel = _inav->get_velocity const Vector3f &curr_vel = _inav->get_velocity const Vector3f &vel_curr = _inav->get_velocity "
ArduPilot 461 AC_WPNav.cpp Non-Control bb5cf4a311 AC_WPNav: make member pointer to AP_InertialNav object const since it snever modified " AC_WPNav::AC_WPNav const AP_InertialNav* inav AP_AHRS* ahrs APM_PI* pid_pos_lat APM_PI* pid_pos_lon AC_PID* pid_rate_lat AC_PID* pid_rate_lon :"
ArduPilot 462 AC_WPNav.cpp Non-Control c3309d909c AC_WPNav: make more member pointers const " AC_WPNav::AC_WPNav const AP_InertialNav* inav const AP_AHRS* ahrs APM_PI* pid_pos_lat APM_PI* pid_pos_lon AC_PID* pid_rate_lat AC_PID* pid_rate_lon :"
ArduPilot 463 AC_WPNav.cpp Control 30fffa5854 AC_WPNav: fixed some build warnings " if dt >= 1.0f dt = 0.0f _limited_speed_xy_cms += 2.0f * _track_accel * dt if dt >= 1.0f if dt == 0.0f _desired_roll = constrain_float fast_atan accel_right*_cos_pitch/ -z_accel_meas * 18000/M_PI_F -_lean_angle_max_cd _lean_angle_max_cd _desired_pitch = constrain_float fast_atan -accel_forward/ -z_accel_meas * 18000/M_PI_F -_lean_angle_max_cd _lean_angle_max_cd "
ArduPilot 464 AC_WPNav.cpp Control 952afd296e Copter: initialise waypoint leash lengthWithout this initialisation the first RTL could be too aggressive as ittries to stop too suddenly " calculate_wp_leash_length true "
ArduPilot 465 AC_WPNav.cpp Control 5767aa47d9 AC_WPNav: reduce leash length for stoppingWe now limit the target stopping point to 1x the xy leash length whilepreviously it was 2x. This is justified because this limit is only usedwhen the copter is travelling at higher speeds but at higher speeds airdrag tends to make the copter stop more quickly naturally. " target_dist = constrain_float target_dist 0 _wp_leash_xy "
ArduPilot 466 AC_WPNav.cpp Control c9415a08f1 AC_WPNav: use ahrs trig values " target_vel_adj.x = _pilot_vel_forward_cms*_ahrs->cos_yaw - _pilot_vel_right_cms*_ahrs->sin_yaw target_vel_adj.y = _pilot_vel_forward_cms*_ahrs->sin_yaw + _pilot_vel_right_cms*_ahrs->cos_yaw accel_forward = accel_lat*_ahrs->cos_yaw + accel_lon*_ahrs->sin_yaw accel_right = -accel_lat*_ahrs->sin_yaw + accel_lon*_ahrs->cos_yaw _desired_roll = constrain_float fast_atan accel_right*_ahrs->cos_pitch / -z_accel_meas * 18000/M_PI_F -_lean_angle_max_cd _lean_angle_max_cd "
ArduPilot 467 AC_WPNav.cpp Control 2984e492df AC_WPNav: add reference to AC_PosControlAlso remove requirement to pass in inertial nav position and velocity toinit_loiter method " _pos_control pos_control void AC_WPNav::init_loiter_target _pos_control.init_pos_target _inav->get_position _inav->get_velocity _pilot_vel_forward_cms = 0 _pilot_vel_right_cms = 0 return get_bearing_cd _inav->get_position _pos_control.get_post_target "
ArduPilot 468 AC_WPNav.cpp Control 0c8cbba644 AC_WPNav: remove xy pos controller " AC_WPNav::AC_WPNav const AP_InertialNav* inav const AP_AHRS* ahrs AC_PosControl& pos_control APM_PI* pid_pos_lat APM_PI* pid_pos_lon AC_PID* pid_rate_lat AC_PID* pid_rate_lon : _loiter_step 0 _pilot_accel_fwd_cms 0 _pilot_accel_rgt_cms 0 _wp_last_update 0 _wp_step 0 _track_length 0.0 _track_desired 0.0 _track_accel 0.0 _track_speed 0.0 _track_leash_length 0.0 void AC_WPNav::set_loiter_target const Vector3f& position _pos_control.set_pos_target _inav->get_position _pos_control.set_desired_velocity 0 0 _pos_control.set_speed_xy _loiter_speed_cms _pos_control.set_accel_xy _loiter_accel_cms _pos_control.set_leash_xy _loiter_leash _pilot_accel_fwd_cms = 0 _pilot_accel_rgt_cms = 0 void AC_WPNav::init_loiter_target Vector3f curr_vel = _inav->get_velocity _pos_control.set_pos_target _inav->get_position _pos_control.set_desired_velocity curr_vel.x curr_vel.y _pos_control.set_speed_xy _loiter_speed_cms _pos_control.set_accel_xy _loiter_accel_cms _pos_control.set_leash_xy _loiter_leash _pilot_accel_fwd_cms = 0 _pilot_accel_rgt_cms = 0 void AC_WPNav::calculate_loiter_leash_length _loiter_leash = _pos_control.calc_leash_length_xy _loiter_speed_cms _loiter_accel_cms void AC_WPNav::set_pilot_desired_acceleration float control_roll float control_pitch _pilot_accel_fwd_cms = -control_pitch * _loiter_accel_cms / 4500.0f _pilot_accel_rgt_cms = control_roll * _loiter_accel_cms / 4500.0f void AC_WPNav::get_loiter_stopping_point_xy Vector3f& stopping_point const _pos_control.get_stopping_point_xy stopping_point void AC_WPNav::calc_loiter_desired_velocity float nav_dt Vector2f desired_accel desired_accel.x = _pilot_accel_fwd_cms*_ahrs->cos_yaw - _pilot_accel_rgt_cms*_ahrs->sin_yaw desired_accel.y = _pilot_accel_fwd_cms*_ahrs->sin_yaw + _pilot_accel_rgt_cms*_ahrs->cos_yaw Vector2f desired_vel = _pos_control.get_desired_velocity desired_vel += desired_accel * nav_dt if desired_vel.x > 0 desired_vel.x -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.x/_loiter_speed_cms desired_vel.x = max desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if desired_vel.x < 0 desired_vel.x -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.x/_loiter_speed_cms desired_vel.x = min desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 if desired_vel.y > 0 desired_vel.y -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.y/_loiter_speed_cms desired_vel.y = max desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if desired_vel.y < 0 desired_vel.y -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.y/_loiter_speed_cms desired_vel.y = min desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 float vel_total = safe_sqrt desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y desired_vel.x = _loiter_speed_cms * desired_vel.x/vel_total desired_vel.y = _loiter_speed_cms * desired_vel.y/vel_total _pos_control.set_desired_velocity desired_vel.x desired_vel.y int32_t AC_WPNav::get_loiter_bearing_to_target const return get_bearing_cd _inav->get_position _pos_control.get_pos_target if dt > 0.095f if dt >= 1.0f dt = 0.0 calc_loiter_desired_velocity dt _pos_control.trigger_xy else _pos_control.update_pos_controller true void AC_WPNav::set_wp_destination const Vector3f& destination if _flags.reached_destination && hal.scheduler->millis - _wp_last_update < 1000 _pos_control.get_stopping_point_xy _origin set_wp_origin_and_destination _origin destination void AC_WPNav::set_wp_origin_and_destination const Vector3f& origin const Vector3f& destination bool climb = pos_delta.z >= 0 calculate_wp_leash_length climb _pos_control.set_pos_target origin _track_desired = 0 _flags.fast_waypoint = false void AC_WPNav::get_wp_stopping_point_xy Vector3f& stopping_point const _pos_control.get_stopping_point_xy stopping_point void AC_WPNav::advance_wp_target_along_track float dt _pos_control.set_pos_target _origin + _pos_delta_unit * _track_desired float AC_WPNav::get_wp_distance_to_destination int32_t AC_WPNav::get_wp_bearing_to_destination float dt = now - _wp_last_update / 1000.0f if dt > 0.095f if dt >= 1.0f dt = 0.0 _wp_last_update = now advance_wp_target_along_track dt _pos_control.trigger_xy else _pos_control.update_pos_controller false float althold_kP = _pos_control.althold_kP if speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / althold_kP _wp_leash_z = speed_vert / althold_kP _wp_leash_z = WPNAV_ALT_HOLD_ACCEL_MAX / 2.0*althold_kP*althold_kP + speed_vert*speed_vert / 2*WPNAV_ALT_HOLD_ACCEL_MAX "
ArduPilot 469 AC_WPNav.cpp Control 1596d83d02 AC_WPNav: move leashes to AC_PosControl " _pos_control.set_speed_xy _wp_speed_cms _pos_control.set_accel_xy _wp_accel_cms _pos_control.set_speed_z _wp_speed_down_cms _wp_speed_up_cms _pos_control.calc_leash_length_xy _pos_control.calc_leash_length_z calculate_wp_leash_length float leash_xy = _pos_control.get_leash_xy float leash_z if track_error.z >= 0 leash_z = _pos_control.get_leash_up_z else leash_z = _pos_control.get_leash_down_z track_extra_max = min _track_leash_length* leash_z-track_error_z /leash_z _track_leash_length* leash_xy-track_error_xy /leash_xy void AC_WPNav::calculate_wp_leash_length float speed_z float leash_z if _pos_delta_unit.z >= 0 speed_z = _wp_speed_up_cms leash_z = _pos_control.get_leash_up_z else speed_z = _wp_speed_down_cms leash_z = _pos_control.get_leash_down_z _track_leash_length = _pos_control.get_leash_xy /pos_delta_unit_xy _track_speed = speed_z/pos_delta_unit_z _track_leash_length = leash_z/pos_delta_unit_z else _track_speed = min speed_z/pos_delta_unit_z _wp_speed_cms/pos_delta_unit_xy _track_leash_length = min leash_z/pos_delta_unit_z _pos_control.get_leash_xy /pos_delta_unit_xy hal.console->printf_P PSTR \nTAC:%4.2f S:%4.2f L:%4.2f\n float _track_accel float _track_speed float _track_leash_length float AC_WPNav::get_bearing_cd const Vector3f &origin const Vector3f &destination const float bearing = 9000 + atan2f - destination.x-origin.x destination.y-origin.y * 5729.57795f if bearing < 0 bearing += 36000 return bearing "
ArduPilot 470 AC_WPNav.cpp Non-Control 4cd45e2edf AC_WPNav: remove debug ""
ArduPilot 471 AC_WPNav.cpp Control fc427967ae AC_WPNav: set wp origin to horiz and vert stopping pointAlso bug fix to set pos_control s down speed as a negative number " _pos_control.get_stopping_point_z _origin _pos_control.set_speed_z -_wp_speed_down_cms _wp_speed_up_cms "
ArduPilot 472 AC_WPNav.cpp Non-Control d4e4620159 AC_WPNav: remove unused PID references " AC_WPNav::AC_WPNav const AP_InertialNav* inav const AP_AHRS* ahrs AC_PosControl& pos_control APM_PI* pid_pos_lat :"
ArduPilot 473 AC_WPNav.cpp Control 9e31f0b985 AC_WPNav: use PosControl accessorSaves 2bytes of RAM " AC_WPNav::AC_WPNav const AP_InertialNav* inav const AP_AHRS* ahrs AC_PosControl& pos_control : float kP = _pos_control.get_pos_xy_kP "
ArduPilot 474 AC_WPNav.cpp Control 98bdbb7fed AC_WPNav: set loiter accel to 1/2 of speedBug fix from Leonard.Also fixed some formatting. " _pos_control.set_pos_target _inav->get_position _pos_control.set_desired_velocity 0 0 _pos_control.set_speed_xy _loiter_speed_cms _loiter_accel_cms = _loiter_speed_cms/2.0f _loiter_accel_cms = _loiter_speed_cms/2.0f _loiter_accel_cms = _loiter_speed_cms/2.0f "
ArduPilot 475 AC_WPNav.cpp Control e5e71ce371 WPNav: add spline support " _track_leash_length 0.0 _spline_time 0.0 _spline_vel_scaler 0.0 _spline_slow_down_dist 0.0 _loiter_last_update = now _pos_control.update_pos_controller true Vector3f origin origin = _destination _pos_control.get_stopping_point_xy origin _pos_control.get_stopping_point_z origin set_wp_origin_and_destination origin destination _flags.segment_type = SEGMENT_STRAIGHT _pos_control.update_pos_controller false void AC_WPNav::set_spline_destination const Vector3f& destination bool stopped_at_start spline_segment_end_type seg_end_type const Vector3f& next_destination Vector3f origin if _flags.reached_destination && hal.scheduler->millis - _wp_last_update < 1000 origin = _destination else _pos_control.get_stopping_point_xy origin _pos_control.get_stopping_point_z origin set_spline_origin_and_destination origin destination stopped_at_start seg_end_type next_destination void AC_WPNav::set_spline_origin_and_destination const Vector3f& origin const Vector3f& destination bool stopped_at_start spline_segment_end_type seg_end_type const Vector3f& next_destination bool prev_segment_exists = _flags.reached_destination && hal.scheduler->millis - _wp_last_update < 1000 if stopped_at_start _spline_origin_vel = destination - origin * 0.1f _spline_time = 0.0f _spline_vel_scaler = 0.0f else if prev_segment_exists if _flags.segment_type == SEGMENT_STRAIGHT _spline_origin_vel = _destination - _origin _spline_time = 0.0f _spline_vel_scaler = 0.0f else _spline_origin_vel = _spline_destination_vel if _spline_time > 1.0f && _spline_time < 1.1f _spline_time -= 1.0f else _spline_time = 0.0f _spline_vel_scaler = 0.0f switch seg_end_type case SEGMENT_END_STOP: _spline_destination_vel = destination - origin * 0.1f _flags.fast_waypoint = false break case SEGMENT_END_STRAIGHT: _spline_destination_vel = next_destination - destination _flags.fast_waypoint = true break case SEGMENT_END_SPLINE: _spline_destination_vel = next_destination - origin _flags.fast_waypoint = true break float vel_len = _spline_origin_vel + _spline_destination_vel .length float pos_len = destination - origin .length * 2.0f if vel_len > pos_len float vel_scaling = pos_len / vel_len update_spline_solution origin destination _spline_origin_vel * vel_scaling _spline_destination_vel * vel_scaling else update_spline_solution origin destination _spline_origin_vel _spline_destination_vel _origin = origin _destination = destination _pos_control.set_speed_xy _wp_speed_cms _pos_control.set_accel_xy _wp_accel_cms _pos_control.set_speed_z -_wp_speed_down_cms _wp_speed_up_cms _pos_control.calc_leash_length_xy _pos_control.calc_leash_length_z calculate_wp_leash_length _spline_slow_down_dist = _wp_speed_cms * _wp_speed_cms / 2.0f*_wp_accel_cms _pos_control.set_pos_target origin _flags.reached_destination = false _flags.segment_type = SEGMENT_SPLINE void AC_WPNav::update_spline if _flags.segment_type != SEGMENT_SPLINE return uint32_t now = hal.scheduler->millis float dt = now - _wp_last_update / 1000.0f if dt > 0.095f if dt >= 1.0f dt = 0.0 _wp_last_update = now advance_spline_target_along_track dt _pos_control.trigger_xy else _pos_control.update_pos_controller false void AC_WPNav::update_spline_solution const Vector3f& origin const Vector3f& dest const Vector3f& origin_vel const Vector3f& dest_vel _hermite_spline_solution 0 = origin _hermite_spline_solution 1 = origin_vel _hermite_spline_solution 2 = -origin*3.0f -origin_vel*2.0f + dest*3.0f - dest_vel _hermite_spline_solution 3 = origin*2.0f + origin_vel -dest*2.0f + dest_vel void AC_WPNav::advance_spline_target_along_track float dt if !_flags.reached_destination Vector3f target_pos target_vel calc_spline_pos_vel _spline_time target_pos target_vel float spline_dist_to_wp = _destination - target_pos .length if !_flags.fast_waypoint && spline_dist_to_wp < _spline_slow_down_dist _spline_vel_scaler = safe_sqrt spline_dist_to_wp * 2.0f * _wp_accel_cms else if _spline_vel_scaler < _wp_speed_cms _spline_vel_scaler += _wp_accel_cms* 0.1f if _spline_vel_scaler > _wp_speed_cms _spline_vel_scaler = _wp_speed_cms float spline_time_scale = _spline_vel_scaler/target_vel.length _pos_control.set_pos_target target_pos _spline_time += spline_time_scale*0.1f if _spline_time >= 1.0f _flags.reached_destination = true void AC_WPNav::calc_spline_pos_vel float spline_time Vector3f& position Vector3f& velocity float spline_time_sqrd = spline_time * spline_time float spline_time_cubed = spline_time_sqrd * spline_time position = _hermite_spline_solution 0 + \ _hermite_spline_solution 1 * spline_time + \ _hermite_spline_solution 2 * spline_time_sqrd + \ _hermite_spline_solution 3 * spline_time_cubed velocity = _hermite_spline_solution 1 + \ _hermite_spline_solution 2 * 2.0f * spline_time + \ _hermite_spline_solution 3 * 3.0f * spline_time_sqrd "
ArduPilot 476 AC_WPNav.cpp Control b42b12f7be AC_WPNav: add get_spline_yaw " _spline_slow_down_dist 0.0 _spline_yaw 0.0 _spline_yaw = _ahrs->yaw_sensor _spline_yaw = RadiansToCentiDegrees atan2 target_vel.y target_vel.x "
ArduPilot 477 AC_WPNav.cpp Control 24eb195aa3 WPNav: add yaw control for straight line wp nav " _yaw 0.0 _yaw = get_bearing_cd _origin _destination _yaw = _ahrs->yaw_sensor _yaw = RadiansToCentiDegrees atan2 target_vel.y target_vel.x "
ArduPilot 478 AC_WPNav.cpp Non-Control 197683d539 WPNav: clean up spline comments ""
ArduPilot 479 AC_WPNav.cpp Control f0f3688172 AC_WPNav: rounder corners for short spline segments " float pos_len = destination - origin .length * 4.0f "
ArduPilot 480 AC_WPNav.cpp Control c0458b786a AC_WPNav: check acceleration is non zero " if _wp_accel_cms <= 0 _wp_accel_cms.set_and_save WPNAV_ACCELERATION if _wp_accel_cms <= 0 _wp_accel_cms.set_and_save WPNAV_ACCELERATION "
ArduPilot 481 AC_WPNav.cpp Non-Control 7dfde39e19 AC_WPNav: advance_spline to use dt " _spline_time += spline_time_scale*dt "
ArduPilot 482 AC_WPNav.cpp Control d27ca53a9d AC_WPNav: spline sets origin vel to zero when no prev segmentIssue caught by Michael Oborne " if stopped_at_start || !prev_segment_exists if _flags.segment_type == SEGMENT_STRAIGHT _spline_origin_vel = _destination - _origin _spline_time = 0.0f _spline_vel_scaler = 0.0f else _spline_origin_vel = _spline_destination_vel if _spline_time > 1.0f && _spline_time < 1.1f _spline_time -= 1.0f _spline_time = 0.0f _spline_vel_scaler = 0.0f "
ArduPilot 483 AC_WPNav.cpp Control b15d4379d8 AC_WPNav: use atan2f make methods const " float AC_WPNav::get_wp_distance_to_destination const int32_t AC_WPNav::get_wp_bearing_to_destination const _yaw = RadiansToCentiDegrees atan2f target_vel.y target_vel.x "
ArduPilot 484 AC_WPNav.cpp Control 15da01cf3a AC_WPNav: replace safe_sqrt with pythagorous2 " float vel_total = pythagorous2 desired_vel.x desired_vel.y float track_error_xy = pythagorous2 track_error.x track_error.y float pos_delta_unit_xy = pythagorous2 _pos_delta_unit.x _pos_delta_unit.y "
ArduPilot 485 AC_WPNav.cpp Control 60f522a094 AC_WPNav: set_wp_destination to use current targetPreviously we used the projected stopping point if the vehicle had notreached the destination but this could lead to large jumps in targetposition " if hal.scheduler->millis - _wp_last_update < 1000 origin = _pos_control.get_pos_target else "
ArduPilot 486 AC_WPNav.cpp Non-Control 72d2712c4e AC_WPNav: integrate update_xy_controller name change " _pos_control.update_xy_controller true _pos_control.update_xy_controller false _pos_control.update_xy_controller false "
ArduPilot 487 AC_WPNav.cpp Control d382fa51ee AC_WPNav: run loiter and wp nav at 50hz on Pixhawk " if dt >= WPNAV_LOITER_UPDATE_TIME if dt >= WPNAV_WP_UPDATE_TIME if dt >= WPNAV_WP_UPDATE_TIME "
ArduPilot 488 AC_WPNav.cpp Control 2167dd7d3e AC_WPNav: update target speed immediately " void AC_WPNav::set_loiter_velocity float velocity_cms if velocity_cms >= WPNAV_LOITER_SPEED_MIN _loiter_speed_cms = velocity_cms _pos_control.set_speed_xy _loiter_speed_cms _loiter_accel_cms = _loiter_speed_cms/2.0f _pos_control.set_accel_xy _loiter_accel_cms if _loiter_speed_cms < WPNAV_LOITER_SPEED_MIN _loiter_speed_cms = WPNAV_LOITER_SPEED_MIN void AC_WPNav::set_horizontal_velocity float velocity_cms if _wp_speed_cms >= WPNAV_WP_SPEED_MIN _wp_speed_cms = velocity_cms _pos_control.set_speed_xy _wp_speed_cms "
ArduPilot 489 AC_WPNav.cpp Non-Control 648787a6c8 AC_WPNav: rename some definitions " _loiter_accel_cms WPNAV_LOITER_ACCEL _track_leash_length = WPNAV_LEASH_LENGTH_MIN "
ArduPilot 490 AC_WPNav.cpp Control 559a258ede AC_WPNav: bug fix to limit target point from moving beyond leashAlso pull Z-axis acceleration from position controller instead of using#define " float track_covered Vector3f track_error float track_desired_max float track_leash_slack bool reached_leash_limit = false track_leash_slack = min _track_leash_length* leash_z-track_error_z /leash_z _track_leash_length* leash_xy-track_error_xy /leash_xy if track_leash_slack < 0 track_desired_max = track_covered + track_leash_slack if _track_desired > track_desired_max reached_leash_limit = true if dt > 0 && !reached_leash_limit _limited_speed_xy_cms += 2.0f * _track_accel * dt _limited_speed_xy_cms = constrain_float _limited_speed_xy_cms 0.0f _track_speed if !reached_leash_limit track_desired_temp += _limited_speed_xy_cms * dt _track_accel = _pos_control.get_accel_z /pos_delta_unit_z _track_accel = min _pos_control.get_accel_z /pos_delta_unit_z _wp_accel_cms/pos_delta_unit_xy "
ArduPilot 491 AC_WPNav.cpp Control 5d0476e522 AC_WPNav: add reset_I to init_loiter_target " void AC_WPNav::init_loiter_target bool reset_I _reset_I = reset_I _pos_control.update_xy_controller true _reset_I "
ArduPilot 492 AC_WPNav.cpp Control 4d5b73b968 AC_WPNav: add reset_I to set_loiter_target " void AC_WPNav::set_loiter_target const Vector3f& position bool reset_I _reset_I = reset_I _pos_control.set_pos_target position "
ArduPilot 493 AC_WPNav.cpp Non-Control 7c02a02bd8 AC_WPNav: reset_I flag moved to position controller " if !reset_I _pos_control.keep_xy_I_terms void AC_WPNav::init_loiter_target _pos_control.update_xy_controller true "
ArduPilot 494 AC_WPNav.cpp Control 46fba47c8e AC_WPNav: slow target point s speed near destination " _slow_down_dist 0.0 _flags.slowing_down = false calc_slow_down_distance _wp_speed_cms _wp_accel_cms if !_flags.fast_waypoint float dist_to_dest = _track_length - _track_desired if !_flags.slowing_down && dist_to_dest <= _slow_down_dist _flags.slowing_down = true if _flags.slowing_down _limited_speed_xy_cms = min _limited_speed_xy_cms get_slow_down_speed dist_to_dest _track_accel calc_slow_down_distance _wp_speed_cms _wp_accel_cms if !_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist void AC_WPNav::calc_slow_down_distance float speed_cms float accel_cmss _slow_down_dist = speed_cms * speed_cms / 4.0f*accel_cmss float AC_WPNav::get_slow_down_speed float dist_from_dest_cm float accel_cmss if dist_from_dest_cm <= 0 return WPNAV_WP_TRACK_SPEED_MIN float target_speed = safe_sqrt dist_from_dest_cm * 4.0f * accel_cmss if target_speed < WPNAV_WP_TRACK_SPEED_MIN return WPNAV_WP_TRACK_SPEED_MIN else return target_speed "
ArduPilot 495 AC_WPNav.cpp Control ad99918fee AC_WPNav: recalc leash lengths if set_horizontal_velocity is calledResolves bug in which do-set-speed allowed reducing the speed during themission but not increasing it.Slow down distance is also recalculated.Unnecessary call to calc_wp_leash_length removed fromset_spline_origin_and_destination. " _flags.recalc_wp_leash = true check_wp_leash_length void AC_WPNav::check_wp_leash_length if _flags.recalc_wp_leash calculate_wp_leash_length calc_slow_down_distance _track_speed _track_accel _flags.recalc_wp_leash = false "
ArduPilot 496 AC_WPNav.cpp Non-Control 510c9920a6 AC_WPNav: rename get_horizontal_velocity to get_speed_xyThis new method name is consistent with the equivalent in theAC_PosControl class " void AC_WPNav::set_speed_xy float speed_cms _wp_speed_cms = speed_cms "
ArduPilot 497 AC_WPNav.cpp Control 0103ae2eb0 AC_WPNav: add WPNAV_ACCEL_ZAllows configurable z-axis acceleration during missions " AP_GROUPINFO ACCEL_Z 6 AC_WPNav _wp_accel_z_cms WPNAV_WP_ACCEL_Z_DEFAULT _pos_control.set_accel_z _wp_accel_z_cms _track_accel = _wp_accel_z_cms/pos_delta_unit_z _track_accel = min _wp_accel_z_cms/pos_delta_unit_z _wp_accel_cms/pos_delta_unit_xy "
ArduPilot 498 AC_WPNav.cpp Control 7dd366d84e AC_WPNav: init_loiter sets speed accel before calcing stopping distanceThe stopping distance depends upon the speed and acceleration so thesemust be updated first " Vector3f curr_vel = _inav->get_velocity _pos_control.set_target_to_stopping_point_xy _pos_control.set_desired_velocity curr_vel.x curr_vel.y "
ArduPilot 499 AC_WPNav.cpp Control fd9f8f571f AC_WPNav: replace inav ahrs pointers with references " AC_WPNav::AC_WPNav const AP_InertialNav& inav const AP_AHRS& ahrs AC_PosControl& pos_control : Vector3f curr_vel = _inav.get_velocity desired_accel.x = _pilot_accel_fwd_cms*_ahrs.cos_yaw - _pilot_accel_rgt_cms*_ahrs.sin_yaw desired_accel.y = _pilot_accel_fwd_cms*_ahrs.sin_yaw + _pilot_accel_rgt_cms*_ahrs.cos_yaw return get_bearing_cd _inav.get_position _pos_control.get_pos_target const Vector3f &curr_vel = _inav.get_velocity Vector3f curr_pos = _inav.get_position const Vector3f &curr_vel = _inav.get_velocity Vector3f curr = _inav.get_position return get_bearing_cd _inav.get_position _destination _yaw = _ahrs.yaw_sensor "
ArduPilot 500 AC_WPNav.cpp Control b38c484874 AC_WPNav: add LOIT_JERK parameterLimit accel output from loiter controller.Call new pos_control.init_xy_controller when loiter startsRemove sudden stop when pilot requested acceleration is zeroPair programmed with Randy " AP_GROUPINFO LOIT_JERK 7 AC_WPNav _loiter_jerk_max_cmsss WPNAV_LOITER_JERK_MAX_DEFAULT const Vector3f& curr_pos = _inav.get_position const Vector3f& curr_vel = _inav.get_velocity _pos_control.init_xy_controller _pos_control.set_xy_target curr_pos.x curr_pos.y _loiter_desired_accel.x = _loiter_accel_cms *curr_vel.x/_loiter_speed_cms _loiter_desired_accel.y = _loiter_accel_cms *curr_vel.y/_loiter_speed_cms Vector2f des_accel_diff = desired_accel - _loiter_desired_accel float des_accel_change_total = pythagorous2 des_accel_diff.x des_accel_diff.y float accel_change_max = _loiter_jerk_max_cmsss * nav_dt if des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total _loiter_desired_accel += des_accel_diff desired_vel += _loiter_desired_accel * nav_dt if _pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f desired_vel.x -= _loiter_accel_cms *nav_dt*desired_vel.x/_loiter_speed_cms desired_vel.y -= _loiter_accel_cms *nav_dt*desired_vel.y/_loiter_speed_cms else desired_vel.x -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.x/_loiter_speed_cms if desired_vel.x > 0 desired_vel.x = max desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if desired_vel.x < 0 desired_vel.x = min desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 desired_vel.y -= _loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN *nav_dt*desired_vel.y/_loiter_speed_cms if desired_vel.y > 0 desired_vel.y = max desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt 0 else if desired_vel.y < 0 desired_vel.y = min desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt 0 "
ArduPilot 501 AC_WPNav.cpp Control 091ff91a70 WPNav: add wp_and_spline_init to set speeds an init pos controller " void AC_WPNav::wp_and_spline_init if _wp_accel_cms <= 0 _wp_accel_cms.set_and_save WPNAV_ACCELERATION _pos_control.init_xy_controller _pos_control.set_speed_xy _wp_speed_cms _pos_control.set_accel_xy _wp_accel_cms _pos_control.set_speed_z -_wp_speed_down_cms _wp_speed_up_cms _pos_control.set_accel_z _wp_accel_z_cms _pos_control.calc_leash_length_xy _pos_control.calc_leash_length_z "
ArduPilot 502 AC_WPNav.cpp Control aed5787c1b AC_WPNav: bug fix for loiter init in HybridAC_PosControl::init_xy_controller() has been added to PosControl and iscalled by init_loiter_target.Hybrid is currently using set_loiter_target function to init the loitercontroller. So we have to call init_xy_controller() by set_loiter_targetfunction.What happens otherwise?In AC_PosControl::update_xy_controller we update now withnow = hal.scheduler->millis();and as _last_update_xy_ms has not been updated previously byinit_xy_controller() we just call init_xy_controller().So _dt_xy will be negative and used anyways in all the functions andPID called by update_xy_controller.That will avoid at least _accel_target.x/y to be set to 0 but I m notsure for the high values probably an I_term that is not reset andreached very high value.Or maybe a cast error somewhere... no clue at all " _pos_control.init_xy_controller _loiter_desired_accel.x = 0 _loiter_desired_accel.y = 0 "
ArduPilot 503 AC_WPNav.cpp Control e7b3c00767 AC_WPNav: set_loiter_target uses set_xy_targetLoiter is only a horizontal position controller so it should not set thez-axis position.Moved pos_control.set_speed and accel functions so order matchesinit_loiter_targets function order " _pos_control.set_speed_xy _loiter_speed_cms _loiter_accel_cms = _loiter_speed_cms/2.0f _pos_control.set_accel_xy _loiter_accel_cms _pos_control.set_xy_target position.x position.y "
ArduPilot 504 AC_WPNav.cpp Non-Control 597d5227f5 AC_WPNav: rename set_loiter_target to init_loiter_target " void AC_WPNav::init_loiter_target const Vector3f& position bool reset_I "
ArduPilot 505 AC_WPNav.cpp Control cde7d31dad AC_WPNav: fix divide by zero when origin and dest are same location " if accel_cmss <= 0.0f _slow_down_dist = 0.0f return "
ArduPilot 506 AC_WPNav.cpp Control 0912bec8f5 Spline div zero and leash limit fix " _track_desired += _limited_speed_xy_cms * dt if _track_desired > track_desired_max _track_desired = track_desired_max _limited_speed_xy_cms -= 2.0f * _track_accel * dt _track_desired = constrain_float _track_desired 0 _track_length float _spline_time_scale = _spline_vel_scaler/target_vel.length _spline_time += _spline_time_scale*dt "
ArduPilot 507 AC_WPNav.cpp Control efd6d6dc70 AC_WPNav: spline div by zero fixAlso add check for straight line navigation to ensure speed is notreduced below zero when it hits the leash limitAlso minor formatting changes " _spline_time_scale 0.0 if _track_desired > track_desired_max if _limited_speed_xy_cms < 0.0f _limited_speed_xy_cms = 0.0f float target_vel_length = target_vel.length if target_vel_length != 0.0f _spline_time_scale = _spline_vel_scaler/target_vel_length "
ArduPilot 508 AC_WPNav.cpp Control f23e94707c AC_WPNav: use fast_atan2 for bearing calcs " _yaw = RadiansToCentiDegrees fast_atan2 target_vel.y target_vel.x float bearing = 9000 + fast_atan2 - destination.x-origin.x destination.y-origin.y * 5729.57795f "
ArduPilot 509 AC_WPNav.cpp Control 8bbce7e658 AC_PosControl: freeze feed forward for alt control in Auto " _pos_control.freeze_ff_z "
ArduPilot 510 AC_WPNav.cpp Control ce85d1f6b2 AC_WPNav: use curr pos target as spline originWe only use the current target position as origin if the waypointcontroller is active (i..e has been used in the past 1 second). This isconsistent with how we initialise straight line waypoints " if hal.scheduler->millis - _wp_last_update < 1000 origin = _pos_control.get_pos_target "
ArduPilot 511 AC_WPNav.cpp Control a2f54fdf2c AC_WPNav: smooth waypoint by freezing feed-forward and allowing overshootFirst part of this fix is freezing the position controller s xy-axisfeed foward as we transition to the new segment.Second part is work-around for straight line segments in that we allowthe target point to actually overshoot the end of the segment by up to2m if the segment is a fast waypoint . Ideally we would instead noticethe waypoint has been completed and take any left over time or distanceand move our target along the track towards the next waypoint but thatwould require a much larger change to allow the wpnav lib to hold thenext two waypoints. " _flags.new_wp_destination = true if !_flags.fast_waypoint _track_desired = constrain_float _track_desired 0 _track_length else _track_desired = constrain_float _track_desired 0 _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX if _flags.new_wp_destination _flags.new_wp_destination = false _pos_control.freeze_ff_xy _flags.new_wp_destination = true if _flags.new_wp_destination _flags.new_wp_destination = false _pos_control.freeze_ff_xy _pos_control.freeze_ff_z "
ArduPilot 512 AC_WPNav.cpp Non-Control c9661cfb09 AC_WPNav: integrate set_desired_velocity_xy function name change " _pos_control.set_desired_velocity_xy 0 0 _pos_control.set_desired_velocity_xy curr_vel.x curr_vel.y Vector3f desired_vel = _pos_control.get_desired_velocity desired_vel.x += _loiter_desired_accel.x * nav_dt desired_vel.y += _loiter_desired_accel.y * nav_dt _pos_control.set_desired_velocity_xy desired_vel.x desired_vel.y "
ArduPilot 513 AC_WPNav.cpp Control 2b64c511ed AC_WPNav: update yaw only when track is at least 2m " if _track_length >= WPNAV_YAW_DIST_MIN _yaw = get_bearing_cd _origin _destination else _yaw = _ahrs.yaw_sensor "
ArduPilot 514 AC_WPNav.cpp Control 3fd2b3b4a1 AC_WPNav: init members to resolve compiler warnings " _track_length 0.0f _track_desired 0.0f _limited_speed_xy_cms 0.0f _track_accel 0.0f _track_speed 0.0f _track_leash_length 0.0f _slow_down_dist 0.0f _spline_time 0.0f _spline_time_scale 0.0f _spline_vel_scaler 0.0f _yaw 0.0f "
ArduPilot 515 AC_WPNav.cpp Control 1c6606cc58 AC_WPNav: resolve twitch when passing spline waypointsThe target positions target velocity was being reset to zero as wepassed through a spline waypoint. " _spline_vel_scaler = _pos_control.get_vel_target .length "
ArduPilot 516 AC_WPNav.cpp Non-Control ad37fc0408 AC_WPNav: WP_SPEED_DN parameter range to 0~500Previous permissible descent speed of 10m/s was unnecessarily lenient.Users can still bypass the suggested range through the MP s fullparameter list if they really want a very high descent speed. ""
ArduPilot 517 AC_WPNav.cpp Control 765420ee04 AC_WPNav: add loiter_soften_for_landing methodThis resets the position target to the current location. " void AC_WPNav::loiter_soften_for_landing const Vector3f& curr_pos = _inav.get_position _pos_control.set_xy_target curr_pos.x curr_pos.y _pos_control.freeze_ff_xy "
ArduPilot 518 AC_WPNav.cpp Control dbe1c55666 AC_WPNav: add shift_wp_origin_to_current_pos for takeoffThis shifts the origin to the vehicle s current position and should becalled just before take-off to ensure there are no sudden roll or pitchmoves on takeoff. " void AC_WPNav::shift_wp_origin_to_current_pos if _track_desired > 0.0f return const Vector3f curr_pos = _inav.get_position const Vector3f pos_target = _pos_control.get_pos_target Vector3f pos_diff = curr_pos - pos_target _origin += pos_diff _destination += pos_diff _pos_control.set_pos_target curr_pos _pos_control.freeze_ff_xy _pos_control.freeze_ff_z "
ArduPilot 519 AC_WPNav.cpp Non-Control e80328d3a5 AC_WPNav: bug fix sanity check of set_speed_xyThis corrects a bug that allowed the waypoint speed to be set to zero " if speed_cms >= WPNAV_WP_SPEED_MIN "
ArduPilot 520 AC_WPNav.cpp Control 369839c7ca AC_WPNav: Use target yaw instead of current yaw for close waypoints " AC_WPNav::AC_WPNav const AP_InertialNav& inav const AP_AHRS& ahrs AC_PosControl& pos_control const AC_AttitudeControl& attitude_control : _attitude_control attitude_control _yaw = _attitude_control.angle_ef_targets .z _yaw = _attitude_control.angle_ef_targets .z "
ArduPilot 521 AC_WPNav.cpp Non-Control 71586d40e3 AC_WPNav: minor fix to comments ""
ArduPilot 522 AC_WPNav.cpp Control 2cc65dffe0 AC_WPNav: add set_spline_dest_and_vel function " void AC_WPNav::set_spline_dest_and_vel const Vector3f& dest_pos const Vector3f& dest_vel if _wp_accel_cms <= 0 _wp_accel_cms.set_and_save WPNAV_ACCELERATION _spline_time = 0.0f _origin = _inav.get_position _destination = dest_pos _spline_origin_vel = _inav.get_velocity _spline_destination_vel = dest_vel if _spline_origin_vel.length < 100.0f _spline_origin_vel = _destination - _origin * 0.1f _spline_vel_scaler = _spline_origin_vel.length _flags.fast_waypoint = _spline_destination_vel.length > 0.0f float vel_len = _spline_origin_vel + _spline_destination_vel .length float pos_len = _destination - _origin .length * 4.0f if vel_len > pos_len float vel_scaling = pos_len / vel_len update_spline_solution _origin _destination _spline_origin_vel * vel_scaling _spline_destination_vel * vel_scaling else update_spline_solution _origin _destination _spline_origin_vel _spline_destination_vel _yaw = _attitude_control.angle_ef_targets .z calc_slow_down_distance _wp_speed_cms _wp_accel_cms _pos_control.set_pos_target _origin _flags.reached_destination = false _flags.segment_type = SEGMENT_SPLINE _flags.new_wp_destination = true "
ArduPilot 523 AC_WPNav.cpp Control 5fa0c59310 AC_WPNav: Add EKF ground speed limit to loiter speed controlThis is required to prevent the speed controller saturating the optical flow sensor during low altitude flying. " void AC_WPNav::calc_loiter_desired_velocity float nav_dt float ekfGndSpdLimit ekfGndSpdLimit = 100.0f * max ekfGndSpdLimit 0.1f float horizSpdDem = sqrtf sq desired_vel.x + sq desired_vel.y if horizSpdDem > ekfGndSpdLimit desired_vel.x = desired_vel.x * ekfGndSpdLimit / horizSpdDem desired_vel.y = desired_vel.y * ekfGndSpdLimit / horizSpdDem void AC_WPNav::update_loiter float ekfGndSpdLimit calc_loiter_desired_velocity dt ekfGndSpdLimit "
ArduPilot 524 AC_WPNav.cpp Non-Control 11fb51ceba AC_WPNav: Add nav velocity gain scaler to interfacesAllows gains to be adjusted to compensate for optical flow noise " void AC_WPNav::update_loiter float ekfGndSpdLimit float ekfNavVelGainScaler _pos_control.update_xy_controller true ekfNavVelGainScaler _pos_control.update_xy_controller false 1.0f _pos_control.update_xy_controller false 1.0f "
ArduPilot 525 AC_WPNav.cpp Control 4a1ba9b186 AC_WPNav: minor const fix " if _pos_delta_unit.z >= 0.0f if pos_delta_unit_z == 0.0f && pos_delta_unit_xy == 0.0f else if _pos_delta_unit.z == 0.0f else if pos_delta_unit_xy == 0.0f "
ArduPilot 526 AC_WPNav.cpp Control 5438d38df5 AC_WPNav: provide reset_I to init_xy_controller " _pos_control.init_xy_controller reset_I "
ArduPilot 527 AC_WPNav.cpp Control 6eab698e85 AC_WPNav: remove various timing hacks " float dt = _pos_control.time_since_last_xy_update if dt >= _pos_control.get_dt_xy if dt >= 0.2f dt = 0.0f float dt = _pos_control.time_since_last_xy_update if dt >= _pos_control.get_dt_xy if dt >= 0.2f dt = 0.0f _pos_control.update_xy_controller false 1.0f _wp_last_update = hal.scheduler->millis float dt = _pos_control.time_since_last_xy_update if dt >= _pos_control.get_dt_xy if dt >= 0.2f dt = 0.0f _wp_last_update = hal.scheduler->millis "
ArduPilot 528 AC_WPNav.cpp Non-Control 626521c366 AC_WPNav: update usage of update_xy_controller " _pos_control.update_xy_controller AC_PosControl::XY_MODE_SLOW_POS_AND_VEL ekfNavVelGainScaler _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_ONLY 1.0f _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_ONLY 1.0f "
ArduPilot 529 AC_WPNav.cpp Non-Control fc4442bf61 AC_WPNav: rename xy_mode " _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF ekfNavVelGainScaler "
ArduPilot 530 AC_WPNav.cpp Control 1da410a6c6 AC_WPNav: allow user to tune loiter " AP_GROUPINFO LOIT_MAXA 8 AC_WPNav _loiter_accel_cmss WPNAV_LOITER_ACCEL AP_GROUPINFO LOIT_MINA 9 AC_WPNav _loiter_accel_min_cmss WPNAV_LOITER_ACCEL_MIN _pos_control.set_accel_xy _loiter_accel_cmss _pos_control.set_accel_xy _loiter_accel_cmss _loiter_desired_accel.x = _loiter_accel_cmss *curr_vel.x/_loiter_speed_cms _loiter_desired_accel.y = _loiter_accel_cmss *curr_vel.y/_loiter_speed_cms _pos_control.set_accel_xy _loiter_accel_cmss _pilot_accel_fwd_cms = -control_pitch * _loiter_accel_cmss / 4500.0f _pilot_accel_rgt_cms = control_roll * _loiter_accel_cmss / 4500.0f _pos_control.set_speed_xy _loiter_speed_cms _pos_control.set_accel_xy _loiter_accel_cmss if _loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f desired_vel.x -= _loiter_accel_cmss *nav_dt*desired_vel.x/_loiter_speed_cms desired_vel.y -= _loiter_accel_cmss *nav_dt*desired_vel.y/_loiter_speed_cms desired_vel.x -= _loiter_accel_cmss-_loiter_accel_min_cmss *nav_dt*desired_vel.x/_loiter_speed_cms desired_vel.x = max desired_vel.x - _loiter_accel_min_cmss*nav_dt 0 desired_vel.x = min desired_vel.x + _loiter_accel_min_cmss*nav_dt 0 desired_vel.y -= _loiter_accel_cmss-_loiter_accel_min_cmss *nav_dt*desired_vel.y/_loiter_speed_cms desired_vel.y = max desired_vel.y - _loiter_accel_min_cmss*nav_dt 0 desired_vel.y = min desired_vel.y + _loiter_accel_min_cmss*nav_dt 0 "
ArduPilot 531 AC_WPNav.cpp Control fd55068620 AC_WPNav: fix double-twitch on stop in loiter " const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity Vector2f desired_vel desired_vel_3d.x desired_vel_3d.y float desired_speed = desired_vel.length if desired_speed != 0.0f Vector2f desired_vel_norm = desired_vel/desired_speed float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/_loiter_speed_cms if _pilot_accel_fwd_cms == 0.0f && _pilot_accel_rgt_cms == 0.0f drag_speed_delta = min drag_speed_delta -_loiter_accel_min_cmss*nav_dt desired_speed = max desired_speed+drag_speed_delta 0.0f desired_vel = desired_vel_norm*desired_speed "
ArduPilot 532 AC_WPNav.cpp Non-Control 39213584da AC_WPNav: minor comment and formatting changes ""
ArduPilot 533 AC_WPNav.cpp Control 4a7fe83b0f AC_WPNav: Create Stop Mode functions " void AC_WPNav::init_stop_target float accel_cmss const Vector3f& curr_vel = _inav.get_velocity Vector3f stopping_point _pos_control.init_xy_controller _pos_control.set_speed_xy curr_vel.length _pos_control.set_accel_xy accel_cmss _pos_control.calc_leash_length_xy _pos_control.get_stopping_point_xy stopping_point init_loiter_target stopping_point void AC_WPNav::update_stop float ekfGndSpdLimit float ekfNavVelGainScaler float dt = _pos_control.time_since_last_xy_update if dt >= _pos_control.get_dt_xy _pos_control.set_desired_velocity_xy 0 0 _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF ekfNavVelGainScaler "
ArduPilot 534 AC_WPNav.cpp Non-Control 35874292a0 AC_WPNav: move stop below all loiter methodsNo functional change " void AC_WPNav::init_stop_target float accel_cmss const Vector3f& curr_vel = _inav.get_velocity Vector3f stopping_point _pos_control.init_xy_controller _pos_control.set_speed_xy curr_vel.length _pos_control.set_accel_xy accel_cmss _pos_control.calc_leash_length_xy _pos_control.get_stopping_point_xy stopping_point init_loiter_target stopping_point void AC_WPNav::update_stop float ekfGndSpdLimit float ekfNavVelGainScaler float dt = _pos_control.time_since_last_xy_update if dt >= _pos_control.get_dt_xy _pos_control.set_desired_velocity_xy 0 0 _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF ekfNavVelGainScaler "
ArduPilot 535 AC_WPNav.cpp Control 7481217445 AC_WPNav: Improve application of EKF optical flow speed limit " float gnd_speed_limit_cms = min _loiter_speed_cms ekfGndSpdLimit*100.0f if gnd_speed_limit_cms < WPNAV_LOITER_SPEED_MIN gnd_speed_limit_cms = WPNAV_LOITER_SPEED_MIN _pos_control.set_speed_xy gnd_speed_limit_cms float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms if horizSpdDem > gnd_speed_limit_cms desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem "
ArduPilot 536 AC_WPNav.cpp Control 33431acaa4 AC_WPNav: protect against div-by-zero related to gnd_speed_limit " gnd_speed_limit_cms = max gnd_speed_limit_cms 10.0f "
ArduPilot 537 AC_WPNav.cpp Non-Control 2b29060a4e AC_WPNav: remove unused set_loiter_velocity ""
ArduPilot 538 AC_WPNav.cpp Control 913d00f525 AP_WPNav: compiler warnings: apply is_zero(float) or is_equal(float) " if !AP_Math::is_zero desired_speed if _pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0 if AP_Math::is_zero _track_length if AP_Math::is_zero pos_delta_unit_z &&AP_Math::is_zero pos_delta_unit_xy else if AP_Math::is_zero _pos_delta_unit.z else if AP_Math::is_zero pos_delta_unit_xy if !AP_Math::is_zero target_vel_length "
ArduPilot 539 AC_WPNav.cpp Non-Control 1b84bbc3e7 AC_WPNav: Compiler warnings: add in the appropriate whitrespace " if AP_Math::is_zero pos_delta_unit_z && AP_Math::is_zero pos_delta_unit_xy "
ArduPilot 540 AC_WPNav.cpp Control 7fd285f483 AC_WPNav: Compiler warnings: nuke fast_atan2()per Randy s suggestion fast_atan2() is no longer necessary over atan2() because only copter uses it and copter is no longer supported on future builds of APMhttps://github.com/Airphrame/ardupilot/commit/ccd578664ffdb5998d2655bd020ae48e7ea628de#commitcomment-11025083 " _yaw = RadiansToCentiDegrees atan2f target_vel.y target_vel.x float bearing = 9000 + atan2f - destination.x-origin.x destination.y-origin.y * 5729.57795f "
ArduPilot 541 AC_WPNav.cpp Control c08b62f9e4 AC_WPNAV: revert AP_Math class change " if !is_zero desired_speed if is_zero _track_length if is_zero pos_delta_unit_z && is_zero pos_delta_unit_xy else if is_zero _pos_delta_unit.z else if is_zero pos_delta_unit_xy if !is_zero target_vel_length "
ArduPilot 542 AC_WPNav.cpp Control 7056484ef0 Copter: fix spline overshoot " float vel_len = _spline_origin_vel.length + _spline_destination_vel.length float vel_len = _spline_origin_vel.length + _spline_destination_vel.length "
ArduPilot 543 AC_WPNav.cpp Non-Control 61c851885f AC_WPNav: remove unused set_spline_dest_and_vel ""
ArduPilot 544 AC_WPNav.cpp Non-Control 0077cac1b7 AC_WPNav: rename stop to brake " void AC_WPNav::init_brake_target float accel_cmss void AC_WPNav::update_brake float ekfGndSpdLimit float ekfNavVelGainScaler "
ArduPilot 545 AC_WPNav.cpp Control aa7a151fe5 AC_WPNav: init flagsResolves Coverity warning " _flags.reached_destination = false _flags.fast_waypoint = false _flags.slowing_down = false _flags.recalc_wp_leash = false _flags.new_wp_destination = false _flags.segment_type = SEGMENT_STRAIGHT "
ArduPilot 546 AC_WPNav.cpp Control 8195f45d6c AC_WPNav: fix spline height loss " _spline_origin_vel = destination - origin * 0.02f _spline_destination_vel = destination - origin * 0.02f _pos_delta_unit = target_vel/target_vel.length calculate_wp_leash_length Vector3f curr_pos = _inav.get_position Vector3f track_error = curr_pos - target_pos float track_error_xy = pythagorous2 track_error.x track_error.y float track_error_z = fabsf track_error.z float leash_xy = _pos_control.get_leash_xy float leash_z if track_error.z >= 0 leash_z = _pos_control.get_leash_up_z else leash_z = _pos_control.get_leash_down_z float track_leash_slack = min _track_leash_length* leash_z-track_error_z /leash_z _track_leash_length* leash_xy-track_error_xy /leash_xy if track_leash_slack < 0.0f track_leash_slack = 0.0f else if _spline_vel_scaler < min _wp_speed_cms track_leash_slack/0.02f _spline_vel_scaler += _wp_accel_cms* 0.02f _spline_vel_scaler = constrain_float _spline_vel_scaler 0.0f min _wp_speed_cms track_leash_slack/0.02f "
ArduPilot 547 AC_WPNav.cpp Control fb9cc124c8 AC_WPNav: replace hardcoded 0.02 with pos_control dt " float dt = _pos_control.get_dt_xy _spline_origin_vel = destination - origin * dt _spline_destination_vel = destination - origin * dt float vel_limit = _wp_speed_cms if !is_zero dt vel_limit = min vel_limit track_leash_slack/dt else if _spline_vel_scaler < vel_limit _spline_vel_scaler += _wp_accel_cms * dt _spline_vel_scaler = constrain_float _spline_vel_scaler 0.0f vel_limit "
ArduPilot 548 AC_WPNav.cpp Non-Control ee2c388bb0 AC_WPNav: standardize inclusion of libaries headersThis commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to so the notation #include is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source then we use the notation #include <> with the path relative to libraries folder.Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time. " #include #include AC_WPNav.h"
ArduPilot 549 AC_WPNav.cpp Control f6f2973acd AC_WPNav: make changes in WPNAV_ACCEL take effect immediatelythis makes it easier to tune the waypoint controller in auto mode " _pos_control.set_accel_xy _wp_accel_cms _pos_control.set_accel_z _wp_accel_z_cms "
ArduPilot 550 AC_WPNav.cpp Control 7706102d1e AC_WPNav: add shift_loiter_target methodShift the loiter target and freeze the feedforward if necessary " void AC_WPNav::shift_loiter_target const Vector3f &pos_adjustment Vector3f new_target = _pos_control.get_pos_target + pos_adjustment _pos_control.set_xy_target new_target.x new_target.y if fabsf pos_adjustment.x > 0.0f || fabsf pos_adjustment.y > 0.0f _pos_control.freeze_ff_xy "
ArduPilot 551 AC_WPNav.cpp Control 5ab2a19173 AC_WPNav: loiter limits lean angle for alt loss " _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF ekfNavVelGainScaler true _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF ekfNavVelGainScaler false _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_ONLY 1.0f false _pos_control.update_xy_controller AC_PosControl::XY_MODE_POS_ONLY 1.0f false "
ArduPilot 552 AC_WPNav.cpp Control 853f8bfaf4 AC_WPNav: stop gradually in loiter " drag_speed_delta = min drag_speed_delta max -_loiter_accel_min_cmss*nav_dt -2.0f*desired_speed*nav_dt "
ArduPilot 553 AC_WPNav.cpp Control 49c4a133d7 AC_WPNav: set jerk separately for wpnav and loiter " _pos_control.set_jerk_xy _loiter_jerk_max_cmsss _pos_control.set_jerk_xy _loiter_jerk_max_cmsss _pos_control.set_jerk_xy _loiter_jerk_max_cmsss _pos_control.set_jerk_xy _loiter_jerk_max_cmsss _pos_control.set_jerk_xy_to_default _pos_control.set_jerk_xy_to_default "
ArduPilot 554 AC_WPNav.cpp Non-Control c52f16b86f AC_WPNav: increase param description max for LOIT_JERK ""
ArduPilot 555 AC_WPNav.cpp Control 040ec481f4 AC_WPNav: loiter uses pos_control.shift_pos_xy_target " _pos_control.shift_pos_xy_target pos_adjustment.x pos_adjustment.y "
ArduPilot 556 AC_WPNav.cpp Control adee13d729 AC_WPNav: handle ekf position reset in Loiter and Brake " _loiter_ekf_pos_reset_ms 0 init_ekf_position_reset check_for_ekf_position_reset init_ekf_position_reset void AC_WPNav::init_ekf_position_reset Vector2f pos_shift _loiter_ekf_pos_reset_ms = _ahrs.getLastPosNorthEastReset pos_shift void AC_WPNav::check_for_ekf_position_reset Vector2f pos_shift uint32_t reset_ms = _ahrs.getLastPosNorthEastReset pos_shift if reset_ms != _loiter_ekf_pos_reset_ms _pos_control.shift_pos_xy_target pos_shift.x * 100.0f pos_shift.y * 100.0f _loiter_ekf_pos_reset_ms = reset_ms "
ArduPilot 557 AC_WPNav.cpp Non-Control 831d8acca5 Remove use of PROGMEMNow variables don t have to be declared with PROGMEM anymore so removethem. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i s/ PROGMEM / /g git grep -l -z PROGMEM | xargs -0 sed -i s/PROGMEM//g The 2 commands were done so we don t leave behind spurious spaces.AVR-specific places were not changed. " const AP_Param::GroupInfo AC_WPNav::var_info = "
ArduPilot 558 AC_WPNav.cpp Control f9c7f15052 AC_WPNav: WPNAV_LOIT_SPEED WPNAV_LOIT_MAXA take effect immediately " _pos_control.set_speed_xy _loiter_speed_cms _pos_control.set_accel_xy _loiter_accel_cmss _pos_control.set_jerk_xy _loiter_jerk_max_cmsss "
ArduPilot 559 AC_WPNav.cpp Control e867a06383 AC_WPNav: use millis/micros/panic functions " if AP_HAL::millis - _wp_last_update < 1000 _wp_last_update = AP_HAL::millis if AP_HAL::millis - _wp_last_update < 1000 bool prev_segment_exists = _flags.reached_destination && AP_HAL::millis - _wp_last_update < 1000 _wp_last_update = AP_HAL::millis "
ArduPilot 560 AC_WPNav.cpp Non-Control 2591261af6 Global: rename min and max macros to uppercaseThe problem with using min() and max() is that they conflict with someC++ headers. Name the macros in uppercase instead. We may go case bycase later converting them to be typesafe.Changes generated with:git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)max(/\1MAX(/g git ls-files *.cpp *.h -z | xargs -0 sed -i s/\([^_[:alnum:]]\)min(/\1MIN(/g " float gnd_speed_limit_cms = MIN _loiter_speed_cms ekfGndSpdLimit*100.0f gnd_speed_limit_cms = MAX gnd_speed_limit_cms 10.0f drag_speed_delta = MIN drag_speed_delta MAX -_loiter_accel_min_cmss*nav_dt -2.0f*desired_speed*nav_dt desired_speed = MAX desired_speed+drag_speed_delta 0.0f track_leash_slack = MIN _track_leash_length* leash_z-track_error_z /leash_z _track_leash_length* leash_xy-track_error_xy /leash_xy _limited_speed_xy_cms = MIN _limited_speed_xy_cms get_slow_down_speed dist_to_dest _track_accel _track_accel = MIN _wp_accel_z_cms/pos_delta_unit_z _wp_accel_cms/pos_delta_unit_xy _track_speed = MIN speed_z/pos_delta_unit_z _wp_speed_cms/pos_delta_unit_xy _track_leash_length = MIN leash_z/pos_delta_unit_z _pos_control.get_leash_xy /pos_delta_unit_xy float track_leash_slack = MIN _track_leash_length* leash_z-track_error_z /leash_z _track_leash_length* leash_xy-track_error_xy /leash_xy vel_limit = MIN vel_limit track_leash_slack/dt "
ArduPilot 561 AC_WPNav.cpp Non-Control 3c4d226b64 AC_WPNav: call renamed functions in AC_AttitudeControl " _yaw = _attitude_control.get_att_target_euler_cd .z _yaw = _attitude_control.get_att_target_euler_cd .z "
ArduPilot 562 AC_WPNav.cpp Control 4908350ccb AC_WPNav: limit WPNAV_ACCEL to that implied by ANGLE_MAXthis prevents an overshoot and backtracking in the navigation codewhen WPNAV_ACCEL is unachievable due to an angle limit " float accel_limit_cms = GRAVITY_MSS * 100 * tanf radians _attitude_control.lean_angle_max *0.01f if _wp_accel_cms > accel_limit_cms _wp_accel_cms.set accel_limit_cms "
ArduPilot 563 AC_WPNav.cpp Control 8b2c479d62 AC_WPNav: straight line waypoints accept terrain " bool AC_WPNav::set_wp_destination const Location_Class& destination bool terr_alt Vector3f dest_neu if !get_vector_NEU destination dest_neu terr_alt return false set_wp_destination dest_neu terr_alt return true bool AC_WPNav::set_wp_destination const Vector3f& destination bool terrain_alt if terrain_alt float origin_terr_offset if !get_terrain_offset origin origin_terr_offset return false origin.z -= origin_terr_offset return set_wp_origin_and_destination origin destination terrain_alt bool AC_WPNav::set_wp_origin_and_destination const Vector3f& origin const Vector3f& destination bool terrain_alt _terrain_alt = terrain_alt float origin_terr_offset = 0.0f if terrain_alt if !get_terrain_offset origin origin_terr_offset return false _pos_control.set_pos_target origin + Vector3f 0 0 origin_terr_offset return true bool AC_WPNav::advance_wp_target_along_track float dt float curr_terr_offset = 0.0f if _terrain_alt && !get_terrain_offset curr_pos curr_terr_offset return false Vector3f curr_delta = curr_pos - Vector3f 0 0 curr_terr_offset - _origin Vector3f final_target = _origin + _pos_delta_unit * _track_desired final_target.z += curr_terr_offset _pos_control.set_pos_target final_target Vector3f dist_to_dest = curr_pos - Vector3f 0 0 curr_terr_offset - _destination return true bool AC_WPNav::update_wpnav bool ret = true if !advance_wp_target_along_track dt ret = false return ret bool AC_WPNav::get_terrain_offset const Vector3f &pos float& offset_cm if !_ekf_origin_terrain_alt_set Location_Class ekforigin = _inav.get_origin int32_t talt_cm if ekforigin.get_alt_cm Location_Class::ALT_FRAME_ABOVE_TERRAIN talt_cm _ekf_origin_terrain_alt_set = true else return false Location_Class curr_loc pos curr_loc.set_alt 0 Location_Class::ALT_FRAME_ABOVE_ORIGIN int32_t pos_terr_alt_cm if curr_loc.get_alt_cm Location_Class::ALT_FRAME_ABOVE_TERRAIN pos_terr_alt_cm offset_cm = -pos_terr_alt_cm return true return false bool AC_WPNav::get_vector_NEU const Location_Class &loc Vector3f &vec bool &terrain_alt Vector3f res_vec if !loc.get_vector_xy_from_origin_NEU res_vec return false if loc.get_alt_frame == Location_Class::ALT_FRAME_ABOVE_TERRAIN int32_t terr_alt if !loc.get_alt_cm Location_Class::ALT_FRAME_ABOVE_TERRAIN terr_alt return false vec.z = terr_alt terrain_alt = true else terrain_alt = false int32_t temp_alt if !loc.get_alt_cm Location_Class::ALT_FRAME_ABOVE_ORIGIN temp_alt return false vec.z = temp_alt terrain_alt = false vec.x = res_vec.x vec.y = res_vec.y return true "
ArduPilot 564 AC_WPNav.cpp Control 9fbfea951a AC_WPNav: spline handles terrain altitudes " bool AC_WPNav::set_spline_destination const Location_Class& destination bool stopped_at_start spline_segment_end_type seg_end_type Location_Class next_destination Vector3f dest_neu bool dest_terr_alt if !get_vector_NEU destination dest_neu dest_terr_alt return false if !next_destination.change_alt_frame destination.get_alt_frame return false Vector3f next_dest_neu bool next_dest_terr_alt if !get_vector_NEU next_destination next_dest_neu next_dest_terr_alt return false return set_spline_destination dest_neu dest_terr_alt stopped_at_start seg_end_type next_dest_neu bool AC_WPNav::set_spline_destination const Vector3f& destination bool terrain_alt bool stopped_at_start spline_segment_end_type seg_end_type const Vector3f& next_destination if terrain_alt float origin_terr_offset if !get_terrain_offset origin origin_terr_offset return false origin.z -= origin_terr_offset return set_spline_origin_and_destination origin destination terrain_alt stopped_at_start seg_end_type next_destination bool AC_WPNav::set_spline_origin_and_destination const Vector3f& origin const Vector3f& destination bool terrain_alt bool stopped_at_start spline_segment_end_type seg_end_type const Vector3f& next_destination _terrain_alt = terrain_alt float origin_terr_offset = 0.0f if terrain_alt if !get_terrain_offset origin origin_terr_offset return false _pos_control.set_pos_target origin + Vector3f 0 0 origin_terr_offset return true bool AC_WPNav::update_spline return false bool ret = true if !advance_spline_target_along_track dt ret = false return ret bool AC_WPNav::advance_spline_target_along_track float dt float curr_terr_offset = 0.0f if _terrain_alt && !get_terrain_offset curr_pos curr_terr_offset return false track_error.z -= curr_terr_offset target_pos.z += curr_terr_offset return true "
ArduPilot 565 AC_WPNav.cpp Non-Control e23c869c5d AC_WPNav: fix reporting of set_wp_destination failure " return set_wp_destination dest_neu terr_alt "
ArduPilot 566 AC_WPNav.cpp Control 1c4b2be16a AC_WPNav: simplify use of terrain to just current location " if !get_terrain_offset origin_terr_offset if !get_terrain_offset origin_terr_offset float terr_offset = 0.0f if _terrain_alt && !get_terrain_offset terr_offset Vector3f curr_delta = curr_pos - Vector3f 0 0 terr_offset - _origin final_target.z += terr_offset Vector3f dist_to_dest = curr_pos - Vector3f 0 0 terr_offset - _destination float terr_offset if !get_terrain_offset terr_offset origin.z -= terr_offset float terr_offset = 0.0f if !get_terrain_offset terr_offset _pos_control.set_pos_target origin + Vector3f 0 0 terr_offset float terr_offset = 0.0f if _terrain_alt && !get_terrain_offset terr_offset track_error.z -= terr_offset target_pos.z += terr_offset bool AC_WPNav::get_terrain_offset float& offset_cm float terr_alt = 0.0f if _terrain != NULL && _terrain->height_above_terrain terr_alt true offset_cm = _inav.get_altitude - terr_alt * 100.0f "
ArduPilot 567 AC_WPNav.cpp Control 41661f815f AP_Math: Replace the pythagorous* functions with a variadic templateThe new function can deal with a variable number of function parameters.Additionally I renamed the functions to norm() because this is thestandard name used in several other projects. " float des_accel_change_total = norm des_accel_diff.x des_accel_diff.y float track_error_xy = norm track_error.x track_error.y return norm _destination.x-curr.x _destination.y-curr.y float pos_delta_unit_xy = norm _pos_delta_unit.x _pos_delta_unit.y float track_error_xy = norm track_error.x track_error.y "
ArduPilot 568 AC_WPNav.cpp Non-Control 684ac12dd6 AC_WPNav: fix compile when using AP_TERRAIN_AVAILABLE 0 " #if AP_TERRAIN_AVAILABLE #endif"
ArduPilot 569 AC_WPNav.cpp Control 2bafc36ded AC_WPNav: support rangefinder for terrain following " if _rangefinder_use if _rangefinder_healthy offset_cm = _inav.get_altitude - _rangefinder_alt_cm return true else return false "
ArduPilot 570 AC_WPNav.cpp Control 8a6aa24525 AC_WPNav: use AC_Avoidance to stop at fence during Loiter " if _avoid != NULL _avoid->adjust_velocity _pos_control.get_pos_xy_kP _loiter_accel_cmss desired_vel "
ArduPilot 571 AC_WPNav.cpp Non-Control b5e80148c6 AC_WPNav: remove shift_loiter_target ""
ArduPilot 572 AC_WPNav.cpp Non-Control 2dc111ff39 Copter: reduce WPNAV_LOIT_MINA parameter description range ""
ArduPilot 573 AC_WPNav.cpp Control c87fa13e77 AC_WPNav: added WP_RFND_USE parameterallows disable of rangefinder use for terrain following " AP_GROUPINFO RFND_USE 10 AC_WPNav _rangefinder_use 1 if _rangefinder_available && _rangefinder_use "
ArduPilot 574 AC_WPNav.cpp Non-Control 152edf7189 Global: remove mode line from headersUsing a global .dir-locals.el file is a better alternative thanreincluding the same emacs header in every file of the project. ""
ArduPilot 575 AC_WPNav.cpp Control c808ee2f49 Global: To nullptr from NULL.RC_Channel: To nullptr from NULL.AC_Fence: To nullptr from NULL.AC_Avoidance: To nullptr from NULL.AC_PrecLand: To nullptr from NULL.DataFlash: To nullptr from NULL.SITL: To nullptr from NULL.GCS_MAVLink: To nullptr from NULL.DataFlash: To nullptr from NULL.AP_Compass: To nullptr from NULL.Global: To nullptr from NULL.Global: To nullptr from NULL. " if _avoid != nullptr if _terrain != nullptr && _terrain->height_above_terrain terr_alt true "
ArduPilot 576 AC_WPNav.cpp Non-Control 6406e34016 AC_WPNav: remove unused _loiter_step member ""
ArduPilot 577 AC_WPNav.cpp Non-Control a32068a973 AC_WPNav: remove ekf position reset handlerThis has been migrated to the position control library ""
ArduPilot 578 AC_WPNav.cpp Non-Control 713b08d830 AC_WPNav: Reduced WPNAV_SPEED minimum to 20cm/s ""
ArduPilot 579 AC_WPNav.cpp Non-Control 17e1329068 AC_WPNav: converted to use AP_AHRS_Viewfor use in quadplane tailsitters " AC_WPNav::AC_WPNav const AP_InertialNav& inav const AP_AHRS_View& ahrs AC_PosControl& pos_control const AC_AttitudeControl& attitude_control :"
ArduPilot 580 AC_WPNav.cpp Control 8c6c2e46cc AC_WPNav: protect against LOIT_SPEED divide-by-zero " _loiter_speed_cms = MAX _loiter_speed_cms WPNAV_LOITER_SPEED_MIN _loiter_speed_cms = MAX _loiter_speed_cms WPNAV_LOITER_SPEED_MIN gnd_speed_limit_cms = MAX gnd_speed_limit_cms WPNAV_LOITER_SPEED_MIN "
ArduPilot 581 AC_WPNav.cpp Non-Control d4c4daca16 AC_WPNav: speed-up and down parameter min to 10cm/s ""
ArduPilot 582 AC_WPNav.cpp Control e96332d346 AC_WPNav: reduce spline to straight twitch " float track_leash_length_abs = fabsf _track_leash_length float track_error_max_abs = MAX fabsf track_error_z fabsf track_error_xy track_leash_slack = track_leash_length_abs > track_error_max_abs ? safe_sqrt sq _track_leash_length - sq track_error_max_abs : 0 track_desired_max = track_covered + track_leash_slack "
ArduPilot 583 AC_WPNav.cpp Non-Control e2cf836734 AC_WPNav: add get_stopping_pointreturns 3D stopping point simply re-uses pos_controller methods " void AC_WPNav::get_wp_stopping_point Vector3f& stopping_point const _pos_control.get_stopping_point_xy stopping_point _pos_control.get_stopping_point_z stopping_point "
ArduPilot 584 AC_WPNav.cpp Control ba011eef78 AC_WPNav: init z-axis feed-foward to correct stopping point calcs " _pos_control.clear_desired_velocity_ff_z "
ArduPilot 585 AC_WPNav.cpp Control 62c123bb08 AC_WPNav: correct straight line waypoint leash calculationThe former calculation was not correctly using the 3D leash " float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z : _pos_control.get_leash_down_z float track_error_max_abs = MAX _track_leash_length*track_error_z/leash_z _track_leash_length*track_error_xy/_pos_control.get_leash_xy "
ArduPilot 586 AC_WPNav.cpp Control b1bd3f0364 AC_WPNav: yaw points along leash " _track_length_xy 0.0f _flags.wp_yaw_set = false _track_length_xy = safe_sqrt sq pos_delta.x +sq pos_delta.y if _track_length_xy >= WPNAV_YAW_DIST_MIN if _pos_control.get_leash_xy < WPNAV_YAW_DIST_MIN set_yaw_cd get_bearing_cd _origin _destination else Vector3f horiz_leash_xy = final_target - curr_pos horiz_leash_xy.z = 0 if horiz_leash_xy.length > MIN WPNAV_YAW_DIST_MIN _pos_control.get_leash_xy *WPNAV_YAW_LEASH_PCT_MIN set_yaw_cd RadiansToCentiDegrees atan2f horiz_leash_xy.y horiz_leash_xy.x float AC_WPNav::get_yaw const if _flags.wp_yaw_set return _yaw else return _attitude_control.get_att_target_euler_cd .z void AC_WPNav::set_yaw_cd float heading_cd _yaw = heading_cd _flags.wp_yaw_set = true _track_length_xy = safe_sqrt sq _destination.x - _origin.x +sq _destination.y - _origin.y if _track_length_xy >= WPNAV_YAW_DIST_MIN if _pos_control.get_leash_xy < WPNAV_YAW_DIST_MIN if !is_zero target_vel.x && !is_zero target_vel.y set_yaw_cd RadiansToCentiDegrees atan2f target_vel.y target_vel.x else float track_error_xy_length = safe_sqrt sq track_error.x +sq track_error.y if track_error_xy_length > MIN WPNAV_YAW_DIST_MIN _pos_control.get_leash_xy *WPNAV_YAW_LEASH_PCT_MIN set_yaw_cd RadiansToCentiDegrees atan2f -track_error.y -track_error.x "
ArduPilot 587 AC_WPNav.cpp Control bffc5daeb0 AC_WPNav: unset yaw when setting new origin and destinationThis ensures that old yaw targets are not used in the short interval before they are initialised in advance_wp_target_along_track or advance_spline_along_track " _flags.wp_yaw_set = false _flags.wp_yaw_set = false "
ArduPilot 588 AC_WPNav.cpp Control a1a5c9f239 AC_WPNav: sanity check wpnav-radiusOccasionally users accidentally set the wpnav-radius to 0 and the vehicle gets stuck at waypoints " _wp_radius_cm = MAX _wp_radius_cm WPNAV_WP_RADIUS_MIN "
ArduPilot 589 AC_WPNav.cpp Non-Control 079d2ff3be AC_WPNav: correct min limit ""
ArduPilot 590 AC_WPNav.cpp Non-Control bd2ba1565c AC_WPNav: minor format fix ""
Paparazzi 1 stabilization_attitude.c Non-Control 363dec8693 Autopilot refactor (#2009)* [autopilot] refactor autopilot API for both firmwares With this fixedwing and rotorcraft are mostly using the same interface for the autopilot. Some specific code and messages handling are still firmware dependent. A large part of the autopilot logic of the fixedwing is moved from main_ap to autopilot_static. More getter/setter functions are provided. * [autopilot] update the rest of the system and the conf for using the refactored autopilot API * [autopilot] fix some errors from CI servers * [actuators] use dummy actuators module to prevent autoloading * Rename Bart_heliDD_INDI.xml to tudelft_bs_helidd_indi.xml * Rename Bart_heliDD_pid.xml to tudelft_bs_helidd_pid.xml * Delete tudelft_course2016_bebop_colorfilter.xml * Delete tudelft_course2016_bebop_avoider.xml * [actuators] don t autoload actuators when set to none * [gcs] autodetect firmware for strip mode button " #include autopilot.h if autopilot.launch "
Paparazzi 2 stabilization_attitude.c Control 75f8e34e51 Skid landing (#1669)Skid landing navigation routine. Comes with an optional landing control for vertical control system I did test the build at a localhost. " if v_ctl_mode == V_CTL_MODE_LANDING h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint else h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs att->phi "
Paparazzi 3 stabilization_attitude.c Non-Control 8edab84dd4 replace TRUE/FALSE by stdbool true/false " h_ctl_disabled = false "
Paparazzi 4 stabilization_attitude.c Non-Control 5c1e4260fe replace bool_t with C99 bool from stdbool.h " bool h_ctl_disabled bool h_ctl_auto1_rate "
Paparazzi 5 stabilization_attitude.c Non-Control faf2206c32 [telemetry] register_periodic: use PPRZ_MSG_ID_x instead of DL_x " register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_CALIBRATION send_calibration "
Paparazzi 6 stabilization_attitude.c Non-Control 83065af402 Change register_periodic_telemetry() to use msg id " register_periodic_telemetry DefaultPeriodic DL_CALIBRATION send_calibration "
Paparazzi 7 stabilization_attitude.c Non-Control 247c0367f8 [state interface] get functions: don t return pointers for single values " float err = stateGetHorizontalSpeedDir_f - h_ctl_course_setpoint float advance = cos err * stateGetHorizontalSpeedNorm_f / reference_advance stateGetHorizontalSpeedNorm_f < reference_advance float speed_depend_nav = stateGetHorizontalSpeedNorm_f / NOMINAL_AIRSPEED err = stateGetAngleOfAttack_f - h_ctl_pitch_loop_setpoint "
Paparazzi 8 stabilization_attitude.c Non-Control f052e69b91 [fixedwing] stabilization_attitude: fix bug in AGR_CLIMBNot sure exactly of the impact as this as lingering in the code for a long time but it was clearly not meant this way... " if v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED "
Paparazzi 9 stabilization_attitude.c Control 908e74ffa7 new setpoints " #if H_CTL_YAW_LOOP float h_ctl_yaw_rate_setpoint pprz_t h_ctl_rudder_setpoint #endif #if H_CTL_CL_LOOP pprz_t h_ctl_flaps_setpoint #endif"
Paparazzi 10 stabilization_attitude.c Non-Control cf96b48d86 [fixedwing] use min/max_cruise_throttle variables instead of definescloses #1057 " float max_dif = Max v_ctl_auto_throttle_max_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle 0.1 float max_dif = Max v_ctl_auto_throttle_nominal_cruise_throttle - v_ctl_auto_throttle_min_cruise_throttle 0.1 "
Paparazzi 11 stabilization_attitude.c Non-Control 603b40a513 [airborne] fix code style on (nearly) all files```find sw/airborne/ -regextype posix-extended -regex sw/airborne/.*(chibios-libopencm3|lpcusb|efsl|lpc21/include|lpc21/test/bootloader|subsystems/ahrs) -prune -o -name *.[ch] -exec ./fix_code_style.sh {} +```ignored ahrs (for now) to not create unnecessary conflicts for some pending changes.. " inline static void h_ctl_roll_loop void inline static void h_ctl_pitch_loop void static inline void h_ctl_roll_rate_loop void static void send_calibration struct transport_tx *trans struct link_device *dev void h_ctl_init void nav_ratio = 0 void h_ctl_course_loop void advance < 1. && *stateGetHorizontalSpeedNorm_f < reference_advance if advance < -0.5 else if advance < 0. err = -advance * 2. * herr else float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain err = Min err half_nav_angle_saturation + h_ctl_course_slew_rate h_ctl_course_slew_rate += h_ctl_course_slew_increment else if err < -half_nav_angle_saturation err = Max err -half_nav_angle_saturation + h_ctl_course_slew_rate h_ctl_course_slew_rate -= h_ctl_course_slew_increment else float speed_depend_nav = *stateGetHorizontalSpeedNorm_f / NOMINAL_AIRSPEED nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_CLIMB_NAV_RATIO 1 nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_DESCENT_NAV_RATIO 1 void h_ctl_attitude_loop void inline static void h_ctl_roll_loop void struct FloatRates *body_rate = stateGetBodyRates_f body_rate->p = err - last_err / 1 / 60. + h_ctl_roll_rate_gain * body_rate->p + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle inline static void h_ctl_roll_loop void + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle static inline void h_ctl_roll_rate_loop if roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES roll_rate_sum_idx = 0 Blend h_ctl_hi_throttle_roll_rate_pgain h_ctl_lo_throttle_roll_rate_pgain v_ctl_throttle_setpoint / float MAX_PPRZ float cmd = throttle_dep_pgain * err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err inline static float loiter void inline static void h_ctl_pitch_loop void struct FloatEulers *att = stateGetNedToBodyEulers_f if h_ctl_elevator_of_roll < 0. switch h_ctl_pitch_mode "
Paparazzi 12 stabilization_attitude.c Non-Control 34b6ca3a26 [telemetry] rename device to link_device " static void send_calibration struct transport_tx *trans struct link_device *dev "
Paparazzi 13 stabilization_attitude.c Non-Control e5cdfa1ce1 [telemetry] convert macros in registered callback to functions " pprz_msg_send_CALIBRATION trans dev AC_ID &v_ctl_auto_throttle_sum_err &v_ctl_auto_throttle_submode "
Paparazzi 14 stabilization_attitude.c Non-Control c4b73552cd [telemetry] change register callback to pass arguments " static void send_calibration struct transport_tx *trans struct device *dev "
Paparazzi 15 stabilization_attitude.c Non-Control 65a82463d4 [fixedwing] move nav.[ch] to firmwares/fixedwing " #include firmwares/fixedwing/nav.h"
Paparazzi 16 stabilization_attitude.c Non-Control eba60ba1fe [telemetry] make specific flag for periodic telemetry " #if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY"
Paparazzi 17 stabilization_attitude.c Non-Control 2be00c735b [fixedwing] cosmetic: untabify " uint8_t h_ctl_pitch_mode nav_ratio=0 advance < 1. && *stateGetHorizontalSpeedNorm_f < reference_advance float err = 0 switch h_ctl_pitch_mode case H_CTL_PITCH_MODE_THETA: err = att->theta - h_ctl_pitch_loop_setpoint break case H_CTL_PITCH_MODE_AOA: err = *stateGetAngleOfAttack_f - h_ctl_pitch_loop_setpoint break default: err = att->theta - h_ctl_pitch_loop_setpoint break err = att->theta - h_ctl_pitch_loop_setpoint "
Paparazzi 18 stabilization_attitude.c Control 116faaeda6 [telemetry] protect telemetry code with #if DOWNLINK- also get rid of AP_DOWNLINK and FBW_DOWNLINK- still needs to make it work again (for the only remaining classix in the world...) " #if DOWNLINK #include subsystems/datalink/telemetry.h #endif #if DOWNLINK #endif"
Paparazzi 19 stabilization_attitude.c Control 1bff48ec2d [telemetry] new periodic telemetry system- only FW currently rotorcraft coming soon- compared to before: - no macros (easier to debug) - the default channel/device cannot be set for all the messages at the same time (it will be possible when we finally replace messages macros and define proper C struct for that) - the register function only register for a single process (not possible to reuse the function for an other process unless you explicitely register for it) " #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif #include subsystems/datalink/downlink.h #include generated/periodic_telemetry.h static void send_calibration void DOWNLINK_SEND_CALIBRATION DefaultChannel DefaultDevice &v_ctl_auto_throttle_sum_err &v_ctl_auto_throttle_submode register_periodic_telemetry DefaultPeriodic CALIBRATION send_calibration "
Paparazzi 20 stabilization_attitude.c Non-Control 49f28d4529 [dox] fix some doxygen file blocks for fixedwing firmware " * @file firmwares/fixedwing/stabilization/stabilization_attitude.c * Fixed wing horizontal control."
Paparazzi 21 stabilization_attitude.c Non-Control d5b53269ba cleanup: remove ident from source files. ""
Paparazzi 22 stabilization_attitude.c Non-Control 40bee12d8a [fixedwing] fix typo in attitude stabilizationwas within an ifdef STRONG_WIND and hence not recognized earlier " float herr = stateGetNedToBodyEulers_f ->psi - h_ctl_course_setpoint "
Paparazzi 23 stabilization_attitude.c Control e9794e8986 [AoA] add angle of attack and sideslip to state interface " err = *stateGetAngleOfAttack_f - h_ctl_pitch_loop_setpoint "
Paparazzi 24 stabilization_attitude.c Non-Control 81bde17609 Merge remote-tracking branch paparazzi/master into state_interfaceConflicts:sw/airborne/subsystems/nav.h ""
Paparazzi 25 stabilization_attitude.c Non-Control ffd52c72a7 [airborne] removed pragma messages warning about changed signs ""
Paparazzi 26 stabilization_attitude.c Non-Control b845da2bb5 Total Energy Control for Fixedwing " #include CTRL_TYPE_H"
Paparazzi 27 stabilization_attitude.c Control aa2cd517e8 [state interface] fw control/guidance/nav convertedOnly the basic navigation routines are converted to the new stateinterfaceSome stuff are missing into the state interface: angle of attack andsideslip angle " #include state.h float err = *stateGetHorizontalSpeedDir_f - h_ctl_course_setpoint float advance = cos err * *stateGetHorizontalSpeedNorm_f / reference_advance *stateGetHorizontalSpeedNorm_f < reference_advance float herr = *stateGetNedToBodyEulers_f ->psi - h_ctl_course_setpoint float speed_depend_nav = *stateGetHorizontalSpeedNorm_f /NOMINAL_AIRSPEED float err = stateGetNedToBodyEulers_f ->phi - h_ctl_roll_setpoint struct FloatRates* body_rate = stateGetBodyRates_f body_rate->p = err - last_err / 1/60. + h_ctl_roll_rate_gain * body_rate->p float err = stateGetNedToBodyEulers_f ->phi - h_ctl_roll_setpoint float err = stateGetBodyRates_f ->p - h_ctl_roll_rate_setpoint struct FloatEulers* att = stateGetNedToBodyEulers_f h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs att->phi err = att->theta - h_ctl_pitch_loop_setpoint err = estimator_AOA - h_ctl_pitch_loop_setpoint err = att->theta - h_ctl_pitch_loop_setpoint err = att->theta - h_ctl_pitch_loop_setpoint "
Paparazzi 28 stabilization_attitude.c Non-Control b90ca9991e fixedwing stabilization attitude: fix sign of v_ctl_altitude_error when using AGR_CLIMB " if v_ctl_altitude_error > 0 "
Paparazzi 29 stabilization_attitude.c Control 81c5792b5c fixedwing gains: do not use ABS anymore print info message that all gains are positive now " #pragma message CAUTION! ALL control gains have to be positive now! h_ctl_course_pgain = H_CTL_COURSE_PGAIN h_ctl_course_dgain = H_CTL_COURSE_DGAIN h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN "
Paparazzi 30 stabilization_attitude.c Non-Control dd15598389 Merge branch dev into positive_control_gainsConflicts:conf/airframes/ENAC/fixed-wing/funjet2.xmlconf/airframes/ENAC/fixed-wing/funjet2_nc.xmlconf/airframes/ENAC/fixed-wing/funjet2_new.xmlconf/airframes/ENAC/fixed-wing/funjet3.xmlconf/airframes/ENAC/fixed-wing/merlin.xmlconf/airframes/ENAC/fixed-wing/minimag1.xmlconf/airframes/ENAC/fixed-wing/obsolete/drops.xmlconf/airframes/ENAC/fixed-wing/overview.xmlconf/airframes/ENAC/fixed-wing/soarzi.xmlconf/airframes/ENAC/fixed-wing/weasel.xmlconf/airframes/LAAS/mmlaas_N1.xmlconf/airframes/LAAS/mmlaas_N2.xmlconf/airframes/LAAS/mmlaas_N3.xmlconf/airframes/Poine/funjet42.xmlconf/airframes/Poine/swift_1.xmlconf/airframes/TU_Delft/MicrojetBR.xmlconf/airframes/delta_wing_minimal_example.xmlconf/airframes/easystar_ets_example.xmlconf/airframes/easystar_example.xmlconf/airframes/example_twog_analogimu.xmlconf/airframes/funjet_cam_example.xmlconf/airframes/funjet_example.xmlconf/airframes/mm/extra/press_t.xmlconf/airframes/mm/extra/probe_t.xmlconf/airframes/mm/extra/turbine_trigger.xmlconf/airframes/mm/fixed-wing/drops.xmlconf/airframes/mm/fixed-wing/funjet43.xmlconf/airframes/mm/fixed-wing/funjetdca.xmlconf/airframes/mm/fixed-wing/funjetdcb.xmlconf/airframes/mm/fixed-wing/funjetdcc.xmlconf/airframes/mm/fixed-wing/funjeteth1.xmlconf/airframes/mm/fixed-wing/funjeteth2.xmlconf/airframes/mm/fixed-wing/funjetfmi1.xmlconf/airframes/mm/fixed-wing/funjetfmi2.xmlconf/airframes/mm/fixed-wing/funjetfmi3.xmlconf/airframes/mm/fixed-wing/funjetgfi1.xmlconf/airframes/mm/fixed-wing/funjetgfi3.xmlconf/airframes/mm/fixed-wing/funjetgfi4.xmlconf/airframes/mm/fixed-wing/funjetgfi5.xmlconf/airframes/mm/fixed-wing/funjetgfi6.xmlconf/airframes/mm/fixed-wing/funjetgfi7.xmlconf/airframes/mm/fixed-wing/funjetgfi8.xmlconf/airframes/mm/fixed-wing/funjetgfi9.xmlconf/airframes/mm/fixed-wing/funjetlisa.xmlconf/airframes/mm/fixed-wing/funjetlisam.xmlconf/airframes/mm/fixed-wing/funjetmm.xmlconf/airframes/mm/fixed-wing/funjetmm2.xmlconf/airframes/mm/fixed-wing/fw_ins_arduimu.xmlconf/airframes/mm/fixed-wing/merlin.xmlconf/airframes/mm/fixed-wing/miniwing.xmlconf/airframes/mm/fixed-wing/slowfast.xmlconf/airframes/mm/fixed-wing/slowfast2.xmlconf/airframes/mm/fixed-wing/twinstarmm.xmlconf/airframes/mm/hangar/black_one.xmlconf/airframes/mm/hangar/glass_one1.xmlconf/airframes/mm/hangar/glass_one2.xmlconf/airframes/mm/hangar/glass_one3.xmlconf/airframes/mm/hangar/lila.xmlconf/airframes/mm/hangar/mac06a.xmlconf/airframes/mm/hangar/red_one.xmlconf/airframes/obsolete/easystar2.xmlconf/airframes/obsolete/kalscott_easystar.xmlconf/airframes/obsolete/microjet5_tp_auto.xmlconf/airframes/obsolete/microjet6.xmlconf/airframes/obsolete/microjetII.xmlconf/airframes/obsolete/minimag1.xmlconf/airframes/obsolete/minimag_fs.xmlconf/airframes/obsolete/mmlaas_N1_carto_cam.xmlconf/airframes/obsolete/storm1.xmlconf/airframes/obsolete/tiny2.xmlconf/airframes/obsolete/xxx1.xmlconf/airframes/test_hb.xmlconf/airframes/twinstar_example.xmlconf/airframes/usb_test.xmlconf/settings/tuning_ctl_adaptive.xmlsw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c ""
Paparazzi 31 stabilization_attitude.c Control 00f8eec9b1 use #if USE_x instead of #ifdef USE_xThen you can also define e.g. -DUSE_GPS=0 and it will work as expected(disable it just like as if it would be undefined)-DUSE_GPS (without a value) has the same effect as -DUSE_GPS=1If you need to define it in the code use #define USE_GPS 1Please always use #if instead of #ifdef if it is a flag to enable/disable somethingand it will always evaluate as an integer arithmetic expression. " #if defined AGR_CLIMB && !USE_AIRSPEED"
Paparazzi 32 stabilization_attitude.c Non-Control b83a9fbd56 Revert #if USE_x instead of #ifdef USE_x somehow this does not work in some cases:arch/lpc21/mcu_periph/adc_arch.c:32:12: error: #if with no expressionThis reverts commit c12f3401a082ef5b016f208a1b1ccd0fec0a3fd6. " #ifdef USE_AOA #ifdef USE_AOA #ifdef USE_AOA"
Paparazzi 33 stabilization_attitude.c Non-Control c12f3401a0 #if USE_x instead of #ifdef USE_x " #if USE_AOA #if USE_AOA #if USE_AOA"
Paparazzi 34 stabilization_attitude.c Control a107375f8e changed h_ctl_roll_rate_gain to positive " h_ctl_roll_rate_gain = ABS H_CTL_ROLL_RATE_GAIN + h_ctl_roll_rate_gain * estimator_p"
Paparazzi 35 stabilization_attitude.c Control 5e59fc1f1b changed h_ctl_roll_attitude_gain to positive " h_ctl_roll_attitude_gain = ABS H_CTL_ROLL_ATTITUDE_GAIN float cmd = h_ctl_roll_attitude_gain * err"
Paparazzi 36 stabilization_attitude.c Control d4d97473d4 changed h_ctl_pitch_dgain to positivealready had postive definition for stabilization_adaptive but negative for stabilization_attitude " h_ctl_pitch_dgain = ABS H_CTL_PITCH_DGAIN "
Paparazzi 37 stabilization_attitude.c Control c3f0bb8170 changed h_ctl_pitch_pgain to positive " h_ctl_pitch_pgain = ABS H_CTL_PITCH_PGAIN h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs estimator_phi float cmd = -h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err "
Paparazzi 38 stabilization_attitude.c Control 225111d9bc start defining all control gains with positive values: changed H_CTL_COURSE_PGAIN to positive " h_ctl_course_pgain = ABS H_CTL_COURSE_PGAIN h_ctl_course_dgain = ABS H_CTL_COURSE_DGAIN #endif float nav_angle_saturation = h_ctl_roll_max_setpoint/h_ctl_course_pgain float cmd = -h_ctl_course_pgain * speed_depend_nav * err + d_err * h_ctl_course_dgain nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_CLIMB_NAV_RATIO 1 nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_DESCENT_NAV_RATIO 1 "
Paparazzi 39 stabilization_attitude.c Non-Control 9b351df8de fix remains of conflict in stabilization_attitude ""
Paparazzi 40 stabilization_attitude.c Non-Control cc5109ee84 Merge pull request #67makes sense so you can t accidentally switch to AOA pitch mode while you don t even have an AOA sensor ""
Paparazzi 41 stabilization_attitude.c Control 38613d1e4a initialize h_ctl_pitch_mode to zero thx Bruzzlee " h_ctl_pitch_mode = 0 "
Paparazzi 42 stabilization_attitude.c Control 1070bb2a54 exclude the AOA code if USE_AOA is not defined " #ifdef USE_AOA uint8_t h_ctl_pitch_mode #endif #ifdef USE_AOA h_ctl_pitch_mode = 0 #endif #ifdef USE_AOA #else err = estimator_theta - h_ctl_pitch_loop_setpoint #endif"
Paparazzi 43 stabilization_attitude.c Control f840fc61f6 AoA Module to use the USDigital M3 as an AoA-SensorPitchloop Switcher in stabilization_attitude.cSettingsfile to load in GUIDownlink message ID 69 " uint8_t h_ctl_pitch_mode float err = 0 switch h_ctl_pitch_mode case H_CTL_PITCH_MODE_THETA: err = estimator_theta - h_ctl_pitch_loop_setpoint break case H_CTL_PITCH_MODE_AOA: err = estimator_AOA - h_ctl_pitch_loop_setpoint break default: err = estimator_theta - h_ctl_pitch_loop_setpoint break "
Paparazzi 44 stabilization_attitude.c Non-Control f0c9716dcd Merge branch master into nav and resolve conflicts ""
Paparazzi 45 stabilization_attitude.c Non-Control 7fd01bcbe2 put generated headers in a seperate generated dir and specifically include generated/xxx.h " #include generated/airframe.h"
Paparazzi 46 stabilization_attitude.c Non-Control c34a790dec fix nav.h includes " #include subsystems/nav.h"
Paparazzi 47 stabilization_attitude.c Non-Control 8de34cc7dd fix some more includes after moving/renaming of fixedwing control " #include firmwares/fixedwing/stabilization/stabilization_attitude.h #include firmwares/fixedwing/guidance/guidance_v.h"
Paparazzi 48 stabilization_attitude.c Non-Control 18f1ad5a0a cleanup some trailing whitespaces " * * Boston MA 02111-1307 USA. /** /** * \brief if else if advance < 0. float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_CLIMB_NAV_RATIO 1 nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_DESCENT_NAV_RATIO 1 h_ctl_aileron_setpoint = TRIM_PPRZ cmd float cmd = h_ctl_roll_pgain * err h_ctl_pitch_setpoint"
Paparazzi 49 stabilization_attitude.c Non-Control 9902669bd1 moved fw_[hv]_ctl to firmwares/fixedwing/stabilization and guidance " #include firmwares/fixedwing/stabilization.h #include firmwares/fixedwing/guidance.h"
Paparazzi 50 fw_h_ctl.c Non-Control 08716bbd8d use double quotes for subsystems and firmware angle brackets only for external includes " #include firmwares/fixedwing/autopilot.h"
Paparazzi 51 fw_h_ctl.c Non-Control 55289d94c2 fixing header path " #include "
Paparazzi 52 fw_h_ctl.c Control f646d52d4c Added aggressive climb mode and climb limiter when USE_AIRSPEED is enabled.Kalman filter parameters have been adjusted for use with the ETS barometric sensor. " #if defined AGR_CLIMB && !defined USE_AIRSPEED "
Paparazzi 53 fw_h_ctl.c Control dfd54d61b9  " float h_ctl_pitch_loop_setpoint h_ctl_pitch_loop_setpoint = 0. h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs estimator_phi float err = estimator_theta - h_ctl_pitch_loop_setpoint float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err "
Paparazzi 54 fw_h_ctl.c Non-Control e508dabace - comments " float h_ctl_course_setpoint "
Paparazzi 55 fw_h_ctl.c Non-Control af5d898020 estimator_psi estimation moved to estimator.c ""
Paparazzi 56 fw_h_ctl.c Control ea5181ead2 Strong Wind " #ifdef STRONG_WIND const float reference_advance = NOMINAL_AIRSPEED / 2. float advance = cos err * estimator_hspeed_mod / reference_advance if advance < 1. && estimator_hspeed_mod < reference_advance float w_vn = cos estimator_hspeed_dir * estimator_hspeed_mod - wind_north float w_ve = sin estimator_hspeed_dir * estimator_hspeed_mod - wind_east estimator_psi = atan2 w_ve w_vn float herr = estimator_psi - h_ctl_course_setpoint NormRadAngle herr if advance < -0.5 err = herr else if advance < 0. err = -advance *2. * herr else err = advance * err #endif"
Paparazzi 57 fw_h_ctl.c Control df16da4f8e Bug fixed: v_ctl_altitude_error may be null ! " if AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0 if v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || V_CTL_AUTO_THROTTLE_BLENDED BoundAbs cmd h_ctl_roll_max_setpoint if v_ctl_altitude_error < 0 nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_CLIMB_NAV_RATIO 1 else nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_DESCENT_NAV_RATIO 1 cmd *= nav_ratio "
Paparazzi 58 fw_h_ctl.c Control 65f1ae5f13 *** empty log message *** " #ifdef SITL static float last_err = 0 estimator_p = err - last_err / 1/60. last_err = err #endif"
Paparazzi 59 fw_h_ctl.c Non-Control dcfc73b70a Cleaned up 2 lines no change in functionality " static float nav_ratio "
Paparazzi 60 fw_h_ctl.c Control 4502bab574 missed the altitude_error " #ifdef AGR_CLIMB float nav_ratio #endif #ifdef AGR_CLIMB nav_ratio=0 #endif v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint if v_ctl_altitude_error < 0 nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END else if v_ctl_altitude_error > 0 nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs v_ctl_altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END "
Paparazzi 61 fw_h_ctl.c Control 7f91efa285 Fixed Error preventing AGR climb from working. Navigation would not blend. " #if defined AGR_CLIMB if AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0 if v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || V_CTL_AUTO_THROTTLE_BLENDED altitude_error = estimator_z - v_ctl_altitude_setpoint BoundAbs cmd h_ctl_roll_max_setpoint if altitude_error < 0 nav_ratio = AGR_CLIMB_NAV_RATIO + 1 - AGR_CLIMB_NAV_RATIO * 1 - fabs altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_CLIMB_NAV_RATIO 1 else if altitude_error > 0 nav_ratio = AGR_DESCENT_NAV_RATIO + 1 - AGR_DESCENT_NAV_RATIO * 1 - fabs altitude_error - AGR_BLEND_END / AGR_BLEND_START - AGR_BLEND_END Bound nav_ratio AGR_DESCENT_NAV_RATIO 1 cmd *= nav_ratio "
Paparazzi 62 fw_h_ctl.c Control 704b878011 Default value for H_CTL_ROLL_RATE_GAIN " #ifndef H_CTL_ROLL_RATE_GAIN #define H_CTL_ROLL_RATE_GAIN 0. #endif"
Paparazzi 63 fw_h_ctl.c Control 835799a1f1 Simpler roll loop (old code will be removed later) " float h_ctl_roll_attitude_gain float h_ctl_roll_rate_gain #ifdef H_CTL_ROLL_PGAIN #endif #ifdef H_CTL_AILERON_OF_THROTTLE #endif #ifdef H_CTL_ROLL_ATTITUDE_GAIN h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN #endif #ifdef H_CTL_ROLL_ATTITUDE_GAIN inline static void h_ctl_roll_loop void float err = estimator_phi - h_ctl_roll_setpoint float cmd = - h_ctl_roll_attitude_gain * err - h_ctl_roll_rate_gain * estimator_p + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle h_ctl_aileron_setpoint = TRIM_PPRZ cmd #else #endif "
Paparazzi 64 fw_h_ctl.c Control 76c599093f course dgain " float h_ctl_course_dgain #ifndef H_CTL_COURSE_DGAIN #define H_CTL_COURSE_DGAIN 0. #endif h_ctl_course_dgain = H_CTL_COURSE_DGAIN static float last_err float d_err = err - last_err last_err = err NormRadAngle d_err float cmd = h_ctl_course_pgain * speed_depend_nav * err + d_err * h_ctl_course_dgain "
Paparazzi 65 fw_h_ctl.c Control 0545439920 course slew " #include autopilot.h #ifdef H_CTL_COURSE_SLEW_INCREMENT float h_ctl_course_slew_increment #endif #ifdef H_CTL_COURSE_SLEW_INCREMENT h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT #endif #ifdef H_CTL_COURSE_SLEW_INCREMENT static float h_ctl_course_slew_rate = 0. float nav_angle_saturation = - h_ctl_roll_max_setpoint/h_ctl_course_pgain float half_nav_angle_saturation = nav_angle_saturation / 2. if launch if err > half_nav_angle_saturation h_ctl_course_slew_rate = Max h_ctl_course_slew_rate 0. err = Min err half_nav_angle_saturation + h_ctl_course_slew_rate h_ctl_course_slew_rate +=h_ctl_course_slew_increment else if err < -half_nav_angle_saturation h_ctl_course_slew_rate = Min h_ctl_course_slew_rate 0. err = Max err -half_nav_angle_saturation + h_ctl_course_slew_rate h_ctl_course_slew_rate -=h_ctl_course_slew_increment else h_ctl_course_slew_rate = 0. #endif"
Paparazzi 66 fw_h_ctl.c Control f509e1660c limited course control in THROTTLE_BLENDED mode (like in AGRESSIVE) " if v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || V_CTL_AUTO_THROTTLE_BLENDED "
Paparazzi 67 fw_h_ctl.c Control ff527c2e4b roll slew " float h_ctl_roll_slew #ifdef H_CTL_ROLL_SLEW h_ctl_roll_slew = H_CTL_ROLL_SLEW #endif float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank #ifdef H_CTL_ROLL_SLEW float diff_roll = roll_setpoint - h_ctl_roll_setpoint BoundAbs diff_roll h_ctl_roll_slew h_ctl_roll_setpoint += diff_roll #else h_ctl_roll_setpoint = roll_setpoint #endif"
Paparazzi 68 fw_h_ctl.c Control a22168ad09 ready for MAV06 " #include std.h float h_ctl_hi_throttle_roll_rate_pgain float h_ctl_lo_throttle_roll_rate_pgain h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN float throttle_dep_pgain = Blend h_ctl_hi_throttle_roll_rate_pgain h_ctl_lo_throttle_roll_rate_pgain v_ctl_throttle_setpoint/ float MAX_PPRZ float cmd = throttle_dep_pgain * err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err "
Paparazzi 69 fw_h_ctl.c Control fb246a9187 *** empty log message *** " #include led.h bool_t h_ctl_auto1_rate if h_ctl_auto1_rate h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10. h_ctl_roll_rate_loop else h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err BoundAbs h_ctl_roll_rate_setpoint H_CTL_ROLL_RATE_MAX_SETPOINT float saved_aileron_setpoint = h_ctl_aileron_setpoint h_ctl_roll_rate_loop h_ctl_aileron_setpoint = Blend h_ctl_aileron_setpoint saved_aileron_setpoint h_ctl_roll_rate_mode "
Paparazzi 70 fw_h_ctl.c Control a4b8424238 *** empty log message *** " elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim "
Paparazzi 71 fw_h_ctl.c Control a46ad658b1 loiter and dash " bool_t h_ctl_disabled float h_ctl_pitch_dgain h_ctl_disabled = FALSE h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN if !h_ctl_disabled h_ctl_roll_loop h_ctl_pitch_loop float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM inline static float loiter void static float last_elevator_trim float throttle_dif = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle if throttle_dif > 0 float max_dif = Max V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle 0.1 elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim else float max_dif = Max v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE 0.1 elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim float max_change = v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim / 80. Bound elevator_trim last_elevator_trim - max_change last_elevator_trim + max_change inline static void h_ctl_pitch_loop void static float last_err if h_ctl_elevator_of_roll <0. h_ctl_elevator_of_roll = 0. float err = estimator_theta - h_ctl_pitch_setpoint float d_err = err - last_err last_err = err float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_elevator_of_roll * fabs estimator_phi #ifdef LOITER_TRIM cmd += loiter #endif h_ctl_elevator_setpoint = TRIM_PPRZ cmd "
Paparazzi 72 fw_h_ctl.c Control 1b25c684f6 *** empty log message *** " float h_ctl_course_pre_bank_correction float h_ctl_roll_rate_setpoint static inline void h_ctl_roll_rate_loop void #endif #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION #define H_CTL_COURSE_PRE_BANK_CORRECTION 1. h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION h_ctl_roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err BoundAbs h_ctl_roll_rate_setpoint H_CTL_ROLL_RATE_MAX_SETPOINT float saved_aileron_setpoint = h_ctl_aileron_setpoint h_ctl_roll_rate_loop h_ctl_aileron_setpoint = Blend h_ctl_aileron_setpoint saved_aileron_setpoint h_ctl_roll_rate_mode static inline void h_ctl_roll_rate_loop float err = estimator_p - h_ctl_roll_rate_setpoint h_ctl_aileron_setpoint = TRIM_PPRZ cmd "
Paparazzi 74 stabilization_attitude_euler_int.c Non-Control 5c1e4260fe replace bool_t with C99 bool from stdbool.h " void stabilization_attitude_read_rc bool in_flight bool in_carefree bool coordinated_turn void stabilization_attitude_run bool in_flight "
Paparazzi 75 stabilization_attitude_euler_int.c Non-Control faf2206c32 [telemetry] register_periodic: use PPRZ_MSG_ID_x instead of DL_x " register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_STAB_ATTITUDE_INT send_att register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT send_att_ref "
Paparazzi 76 stabilization_attitude_euler_int.c Non-Control 83065af402 Change register_periodic_telemetry() to use msg id " register_periodic_telemetry DefaultPeriodic DL_STAB_ATTITUDE_INT send_att register_periodic_telemetry DefaultPeriodic DL_STAB_ATTITUDE_REF_INT send_att_ref "
Paparazzi 77 stabilization_attitude_euler_int.c Non-Control 794384470b [rotorcraft] attitude ref: refactor euler_int " #ifndef USE_ATTITUDE_REF #define USE_ATTITUDE_REF 1 #endif struct Int32Eulers stab_att_sp_euler struct AttRefEulerInt att_ref_euler_i att_ref_euler_i.euler.psi = stab_att_sp_euler.psi << REF_ANGLE_FRAC - INT32_ANGLE_FRAC att_ref_euler_i.rate.r = 0 att_ref_euler_i.accel.r = 0 &att_ref_euler_i.euler.phi &att_ref_euler_i.euler.theta &att_ref_euler_i.euler.psi &att_ref_euler_i.rate.p &att_ref_euler_i.rate.q &att_ref_euler_i.rate.r &att_ref_euler_i.accel.p &att_ref_euler_i.accel.q &att_ref_euler_i.accel.r INT_EULERS_ZERO stab_att_sp_euler attitude_ref_euler_int_init &att_ref_euler_i #if USE_ATTITUDE_REF attitude_ref_euler_int_update &att_ref_euler_i &stab_att_sp_euler #else INT32_EULERS_LSHIFT att_ref_euler_i.euler stab_att_sp_euler REF_ANGLE_FRAC - INT32_ANGLE_FRAC INT_RATES_ZERO att_ref_euler_i.rate INT_RATES_ZERO att_ref_euler_i.accel #endif OFFSET_AND_ROUND stabilization_gains.dd.x * att_ref_euler_i.accel.p 5 OFFSET_AND_ROUND stabilization_gains.dd.y * att_ref_euler_i.accel.q 5 OFFSET_AND_ROUND stabilization_gains.dd.z * att_ref_euler_i.accel.r 5 OFFSET_AND_ROUND att_ref_euler_i.euler.phi REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND att_ref_euler_i.euler.theta REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND att_ref_euler_i.euler.psi REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND att_ref_euler_i.rate.p REF_RATE_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND att_ref_euler_i.rate.q REF_RATE_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND att_ref_euler_i.rate.r REF_RATE_FRAC - INT32_RATE_FRAC "
Paparazzi 78 stabilization_attitude_euler_int.c Non-Control 7322cdbfd0 [airborne] replace memcpy with assignment for structsNo reason to use memcpy to copy structs assignment works since C90.Normal assignment should actually be preferred as it will better deal with alignment/padding in some cases. " stab_att_sp_euler = *rpy "
Paparazzi 79 stabilization_attitude_euler_int.c Non-Control 603b40a513 [airborne] fix code style on (nearly) all files```find sw/airborne/ -regextype posix-extended -regex sw/airborne/.*(chibios-libopencm3|lpcusb|efsl|lpc21/include|lpc21/test/bootloader|subsystems/ahrs) -prune -o -name *.[ch] -exec ./fix_code_style.sh {} +```ignored ahrs (for now) to not create unnecessary conflicts for some pending changes.. " static inline void reset_psi_ref_from_body void static void send_att struct transport_tx *trans struct link_device *dev struct Int32Rates *body_rate = stateGetBodyRates_i struct Int32Eulers *att = stateGetNedToBodyEulers_i & body_rate->p & body_rate->q & body_rate->r & att->phi & att->theta & att->psi &stab_att_sp_euler.phi &stab_att_sp_euler.theta &stab_att_sp_euler.psi &stabilization_att_sum_err.phi &stabilization_att_sum_err.theta &stabilization_att_sum_err.psi &stabilization_att_fb_cmd COMMAND_ROLL &stabilization_att_fb_cmd COMMAND_PITCH &stabilization_att_fb_cmd COMMAND_YAW &stabilization_att_ff_cmd COMMAND_ROLL &stabilization_att_ff_cmd COMMAND_PITCH &stabilization_att_ff_cmd COMMAND_YAW &stabilization_cmd COMMAND_ROLL &stabilization_cmd COMMAND_PITCH &stabilization_cmd COMMAND_YAW static void send_att_ref struct transport_tx *trans struct link_device *dev &stab_att_sp_euler.phi &stab_att_sp_euler.theta &stab_att_sp_euler.psi &stab_att_ref_euler.phi &stab_att_ref_euler.theta &stab_att_ref_euler.psi &stab_att_ref_rate.p &stab_att_ref_rate.q &stab_att_ref_rate.r &stab_att_ref_accel.p &stab_att_ref_accel.q &stab_att_ref_accel.r void stabilization_attitude_init void INT_EULERS_ZERO stabilization_att_sum_err void stabilization_attitude_read_rc bool_t in_flight bool_t in_carefree bool_t coordinated_turn void stabilization_attitude_enter void INT_EULERS_ZERO stabilization_att_sum_err void stabilization_attitude_set_failsafe_setpoint void void stabilization_attitude_set_rpy_setpoint_i struct Int32Eulers *rpy void stabilization_attitude_set_earth_cmd_i struct Int32Vect2 *cmd int32_t heading void stabilization_attitude_run bool_t in_flight OFFSET_AND_ROUND stab_att_ref_euler.phi REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND stab_att_ref_euler.psi REF_ANGLE_FRAC - INT32_ANGLE_FRAC struct Int32Eulers *ltp_to_body_euler = stateGetNedToBodyEulers_i else OFFSET_AND_ROUND stab_att_ref_rate.r REF_RATE_FRAC - INT32_RATE_FRAC struct Int32Rates *body_rate = stateGetBodyRates_i OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_ROLL + stabilization_att_ff_cmd COMMAND_ROLL CMD_SHIFT OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_PITCH + stabilization_att_ff_cmd COMMAND_PITCH CMD_SHIFT OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_YAW + stabilization_att_ff_cmd COMMAND_YAW CMD_SHIFT "
Paparazzi 80 stabilization_attitude_euler_int.c Non-Control 34b6ca3a26 [telemetry] rename device to link_device " static void send_att struct transport_tx *trans struct link_device *dev static void send_att_ref struct transport_tx *trans struct link_device *dev "
Paparazzi 81 stabilization_attitude_euler_int.c Non-Control e5cdfa1ce1 [telemetry] convert macros in registered callback to functions " pprz_msg_send_STAB_ATTITUDE_INT trans dev AC_ID pprz_msg_send_STAB_ATTITUDE_REF_INT trans dev AC_ID "
Paparazzi 82 stabilization_attitude_euler_int.c Non-Control c4b73552cd [telemetry] change register callback to pass arguments " static void send_att struct transport_tx *trans struct device *dev static void send_att_ref struct transport_tx *trans struct device *dev "
Paparazzi 83 stabilization_attitude_euler_int.c Non-Control 0c64a410c0 [rotorcraft] reorg stabilization includes/inittowards not requiring any specfic stabilization controllers (none rate attitude) " #include firmwares/rotorcraft/stabilization/stabilization_attitude.h"
Paparazzi 84 stabilization_attitude_euler_int.c Control 96778c01f3 [rotorcraft] include and dox cleanup " #include generated/airframe.h #include firmwares/rotorcraft/stabilization.h #include std.h #include paparazzi.h #include math/pprz_algebra_int.h #include state.h"
Paparazzi 85 stabilization_attitude_euler_int.c Non-Control 4601197d36 [rotorcraft] error on negative gains " #error ALL control gains have to be positive!!!"
Paparazzi 86 stabilization_attitude_euler_int.c Control f4b8517ed5 [rotorcraft] rc setpoint handling refactoredcloses #684 which partially solves #599 " void stabilization_attitude_read_rc bool_t in_flight bool_t in_carefree bool_t coordinated_turn stabilization_attitude_read_rc_setpoint_eulers &stab_att_sp_euler in_flight in_carefree coordinated_turn "
Paparazzi 87 stabilization_attitude_euler_int.c Non-Control eba60ba1fe [telemetry] make specific flag for periodic telemetry " #if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY"
Paparazzi 88 stabilization_attitude_euler_int.c Non-Control 8ac5fd8d5b Merge branch master into telemetry* master: (125 commits) [abi] define ABI x_SENDER_IDs in abi_sender_ids.h [sim] flight gear viz: daytime on all spots on the planet not only France [abi] split abi messages from telemetry messages [fix] fix typo in actuators asctec v2 [baro] fix baro scaling for booz and navgo [boards] sim pc: only USE_BARO_BOARD for nps [conf] move some airframes to obsolete [modules] add baro_sim [sim] remove unused sim_baro [apogee] fix baro mpl3115 init on apogee Update README for recommended gcc-arm-embedded toolchain [boards] stm32f4_discovery typo in description [boards] roughly correct BOOZ_BARO_SENS [modules] baro_ets: BARO_ETS_SCALE [modules] baro modules for real sensors only for ap target not sim [conf] remove baro_board.xml module file [boards] update stm32f4_discovery [lib/ocaml] update leap_seconds [mission] fix segmentation fault and add generic mission FP [generator] generate arrays of the settings and fp blocks names ...Conflicts:sw/airborne/firmwares/fixedwing/ap_downlink.hsw/airborne/firmwares/fixedwing/main_ap.csw/airborne/firmwares/rotorcraft/telemetry.hsw/airborne/modules/sensors/baro_MS5534A.c ""
Paparazzi 89 stabilization_attitude_euler_int.c Non-Control 3e92315488 Merge remote-tracking branch paparazzi/master into telemetryConflicts:conf/firmwares/subsystems/fixedwing/autopilot.makefilesw/airborne/boards/ardrone/navdata.csw/airborne/boards/lisa_m/baro_ms5611_i2c.csw/airborne/boards/lisa_m/baro_ms5611_spi.csw/airborne/firmwares/fixedwing/ap_downlink.hsw/airborne/firmwares/fixedwing/main_ap.csw/airborne/firmwares/rotorcraft/telemetry.hsw/airborne/mcu_periph/i2c.csw/airborne/subsystems/ahrs/ahrs_float_cmpl.csw/airborne/subsystems/ahrs/ahrs_float_lkf.csw/airborne/subsystems/datalink/downlink.hsw/tools/gen_periodic.ml ""
Paparazzi 90 stabilization_attitude_euler_int.c Control adfeb50c24 [rotorcraft][stabilization] set_rpy|earth_cmd for euler " void stabilization_attitude_set_rpy_setpoint_i struct Int32Eulers *rpy memcpy &stab_att_sp_euler rpy sizeof struct Int32Eulers void stabilization_attitude_set_earth_cmd_i struct Int32Vect2 *cmd int32_t heading int32_t psi = stateGetNedToBodyEulers_i ->psi int32_t s_psi c_psi PPRZ_ITRIG_SIN s_psi psi PPRZ_ITRIG_COS c_psi psi stab_att_sp_euler.phi = -s_psi * cmd->x + c_psi * cmd->y >> INT32_TRIG_FRAC stab_att_sp_euler.theta = - c_psi * cmd->x + s_psi * cmd->y >> INT32_TRIG_FRAC stab_att_sp_euler.psi = heading "
Paparazzi 91 stabilization_attitude_euler_int.c Control 104acd2767 [rotorcraft][guidance][stabilization] quat setpoint fixesDon t pretend that the commands from guidance_h are actually real euler angles.Compose a roll/pitch quaternion from simultaneous rotation of roll/pitch then rotate around resulting body z-axis to align the heading.This should fix the setpoint passed to the attitude stabilization if in HOVER or NAVat large angles.Only tested quickly in simulation... " void stabilization_attitude_set_cmd_i struct Int32Eulers *sp_cmd memcpy &stab_att_sp_euler sp_cmd sizeof struct Int32Eulers "
Paparazzi 92 stabilization_attitude_euler_int.c Non-Control 2b3a1da82e Merge remote-tracking branch paparazzi/master into telemetryConflicts:sw/airborne/firmwares/rotorcraft/autopilot.c ""
Paparazzi 93 stabilization_attitude_euler_int.c Non-Control e4a3316d81 [rotorcraft] add missing include so stabilization_attitude_euler_int can be used standalone " #include paparazzi.h"
Paparazzi 94 stabilization_attitude_euler_int.c Non-Control 116faaeda6 [telemetry] protect telemetry code with #if DOWNLINK- also get rid of AP_DOWNLINK and FBW_DOWNLINK- still needs to make it work again (for the only remaining classix in the world...) " #if DOWNLINK #include subsystems/datalink/telemetry.h #endif #if DOWNLINK #endif"
Paparazzi 95 stabilization_attitude_euler_int.c Control 8377d60184 [telemetry] adapt rotorcraft to new telemetry system " #include subsystems/datalink/downlink.h #include generated/periodic_telemetry.h static void send_att void struct Int32Rates* body_rate = stateGetBodyRates_i struct Int32Eulers* att = stateGetNedToBodyEulers_i DOWNLINK_SEND_STAB_ATTITUDE_INT DefaultChannel DefaultDevice & body_rate->p & body_rate->q & body_rate->r & att->phi & att->theta & att->psi &stab_att_sp_euler.phi &stab_att_sp_euler.theta &stab_att_sp_euler.psi &stabilization_att_sum_err.phi &stabilization_att_sum_err.theta &stabilization_att_sum_err.psi &stabilization_att_fb_cmd COMMAND_ROLL &stabilization_att_fb_cmd COMMAND_PITCH &stabilization_att_fb_cmd COMMAND_YAW &stabilization_att_ff_cmd COMMAND_ROLL &stabilization_att_ff_cmd COMMAND_PITCH &stabilization_att_ff_cmd COMMAND_YAW &stabilization_cmd COMMAND_ROLL &stabilization_cmd COMMAND_PITCH &stabilization_cmd COMMAND_YAW static void send_att_ref void DOWNLINK_SEND_STAB_ATTITUDE_REF_INT DefaultChannel DefaultDevice &stab_att_sp_euler.phi &stab_att_sp_euler.theta &stab_att_sp_euler.psi &stab_att_ref_euler.phi &stab_att_ref_euler.theta &stab_att_ref_euler.psi &stab_att_ref_rate.p &stab_att_ref_rate.q &stab_att_ref_rate.r &stab_att_ref_accel.p &stab_att_ref_accel.q &stab_att_ref_accel.r register_periodic_telemetry DefaultPeriodic STAB_ATTITUDE send_att register_periodic_telemetry DefaultPeriodic STAB_ATTITUDE_REF send_att_ref "
Paparazzi 96 stabilization_attitude_euler_int.c Control 1de769b6f2 [rotorcraft] big fat attempt to clean stabilization and guidance- No more STABILIZATION_ATTITUDE_FLOAT_x in airframe file.- Hopefully seamlessly switch between int and float.- Also make it possible to not use a reference at all e.g. for passthrough " static inline void reset_psi_ref_from_body void stab_att_ref_euler.psi = stab_att_sp_euler.psi << REF_ANGLE_FRAC - INT32_ANGLE_FRAC stab_att_ref_rate.r = 0 stab_att_ref_accel.r = 0 void stabilization_attitude_set_failsafe_setpoint void stab_att_sp_euler.phi = 0 stab_att_sp_euler.theta = 0 stab_att_sp_euler.psi = stateGetNedToBodyEulers_i ->psi void stabilization_attitude_set_from_eulers_i struct Int32Eulers *sp_euler memcpy &stab_att_sp_euler sp_euler sizeof struct Int32Eulers "
Paparazzi 97 stabilization_attitude_euler_int.c Non-Control c57e6c704e [stab att] converte stab attitude euler to new state interface " #include state.h stab_att_sp_euler.psi = stateGetNedToBodyEulers_i ->psi struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i EULERS_DIFF att_err att_ref_scaled *ltp_to_body_euler struct Int32Rates* body_rate = stateGetBodyRates_i RATES_DIFF rate_err rate_ref_scaled *body_rate "
Paparazzi 98 stabilization_attitude_euler_int.c Control 313bde5157 rotorcraft stabilization: updated gain scales to get roughly the same gains as before the change of commands* attitude int_quat: multiply old gain scale by 48 (9600/200)* attitude int euler: changed CMD_SHIFT from 16 to 11 so a multiply of 32 to be a bit more conservative for a start* rate: changed the command shift by 5 so also a conservative 32 " #define CMD_SHIFT 11"
Paparazzi 99 stabilization_attitude_euler_int.c Non-Control 896c84ebec Merge branch attitude_ref_frac into dev* cleaned up rotorcraft stabilization and guidance* removed not properly functioning nav tracking algorithm* same guidance loop for hover and nav (except that nav uses a reference)* guidance should now be running properly for quaternion stabilization as well ""
Paparazzi 100 stabilization_attitude_euler_int.c Non-Control a24ae2d31c Merge branch 4.0_beta into dev ""
Paparazzi 101 stabilization_attitude_euler_int.c Non-Control 577b52a016 rotorcraft guidance and stabilization:* moved reading of RC setpoint to separate header file* fix and clean up resetting of psi reference* more minor cleanup " #include firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h stabilization_attitude_read_rc_setpoint_eulers &stab_att_sp_euler in_flight stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi reset_psi_ref_from_body "
Paparazzi 102 stabilization_attitude_euler_int.c Control 1905e350c4 rotorcraft stabilization: bound stabilization_cmd to MAX_PPRZ " BoundAbs stabilization_cmd COMMAND_ROLL MAX_PPRZ BoundAbs stabilization_cmd COMMAND_PITCH MAX_PPRZ BoundAbs stabilization_cmd COMMAND_YAW MAX_PPRZ "
Paparazzi 103 stabilization_attitude_euler_int.c Non-Control a984fb6eb7 Merge branch dev into 4.0_betacontains fixes for:* stabilization int_quat now works correctly for quads and final command always works (even if pitch command is first command in airframe file)* added sys_time_usleep implementation for stm32 this fixes xbee_api mode again ""
Paparazzi 104 stabilization_attitude_euler_int.c Non-Control 94335cf29e rotorcraft stabilization int quat: removed unused/non-functional gain scheduling ""
Paparazzi 105 stabilization_attitude_euler_int.c Non-Control 6e1072abea rotorcraft stabilization int:* moved common defines to attitude_ref_int.h* replaced STABILIZATION_ATTITUDE_READ_RC macro with stabilization_attitude_read_rc_ref static inline function* removed unused stab_att_sp_vi_euler and stab_att_sp_rc_euler from attitude_ref_int.h " stabilization_attitude_read_rc_ref &stab_att_sp_euler in_flight "
Paparazzi 106 stabilization_attitude_euler_int.c Control df76dc8fbe rotorcraft gains: do not use ABS anymore warn instead if gains are negative " #if STABILIZATION_ATTITUDE_PHI_PGAIN < 0 || \ STABILIZATION_ATTITUDE_THETA_PGAIN < 0 || \ STABILIZATION_ATTITUDE_PSI_PGAIN < 0 || \ STABILIZATION_ATTITUDE_PHI_DGAIN < 0 || \ STABILIZATION_ATTITUDE_THETA_DGAIN < 0 || \ STABILIZATION_ATTITUDE_PSI_DGAIN < 0 || \ STABILIZATION_ATTITUDE_PHI_IGAIN < 0 || \ STABILIZATION_ATTITUDE_THETA_IGAIN < 0 || \ STABILIZATION_ATTITUDE_PSI_IGAIN < 0 #warning ALL control gains are now positive!!! #endif STABILIZATION_ATTITUDE_PHI_PGAIN STABILIZATION_ATTITUDE_THETA_PGAIN STABILIZATION_ATTITUDE_PSI_PGAIN STABILIZATION_ATTITUDE_PHI_DGAIN STABILIZATION_ATTITUDE_THETA_DGAIN STABILIZATION_ATTITUDE_PSI_DGAIN STABILIZATION_ATTITUDE_PHI_IGAIN STABILIZATION_ATTITUDE_THETA_IGAIN STABILIZATION_ATTITUDE_PSI_IGAIN STABILIZATION_ATTITUDE_PHI_DDGAIN STABILIZATION_ATTITUDE_THETA_DDGAIN STABILIZATION_ATTITUDE_PSI_DDGAIN "
Paparazzi 107 stabilization_attitude_euler_int.c Non-Control ea47766125 Merge branch positive_control_gains into rotorcraft_positive_control_gainsConflicts:sw/airborne/math/pprz_algebra.h ""
Paparazzi 108 stabilization_attitude_euler_int.c Non-Control b83a9fbd56 Revert #if USE_x instead of #ifdef USE_x somehow this does not work in some cases:arch/lpc21/mcu_periph/adc_arch.c:32:12: error: #if with no expressionThis reverts commit c12f3401a082ef5b016f208a1b1ccd0fec0a3fd6. " #ifdef USE_HELI"
Paparazzi 109 stabilization_attitude_euler_int.c Non-Control c12f3401a0 #if USE_x instead of #ifdef USE_x " #if USE_HELI"
Paparazzi 110 stabilization_attitude_euler_int.c Non-Control 5d2b11099b readability: defined a CMD_SHIFT for heli.. " #ifdef USE_HELI #define CMD_SHIFT 12 #else #define CMD_SHIFT 16 #endif stabilization_cmd COMMAND_ROLL = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_ROLL +stabilization_att_ff_cmd COMMAND_ROLL CMD_SHIFT OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_PITCH +stabilization_att_ff_cmd COMMAND_PITCH CMD_SHIFT OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_YAW +stabilization_att_ff_cmd COMMAND_YAW CMD_SHIFT "
Paparazzi 111 stabilization_attitude_euler_int.c Control 3167c8fecc changed rotorcraft stabilization_attitude_euler gains to positive " ABS STABILIZATION_ATTITUDE_PHI_PGAIN ABS STABILIZATION_ATTITUDE_THETA_PGAIN ABS STABILIZATION_ATTITUDE_PSI_PGAIN ABS STABILIZATION_ATTITUDE_PHI_DGAIN ABS STABILIZATION_ATTITUDE_THETA_DGAIN ABS STABILIZATION_ATTITUDE_PSI_DGAIN ABS STABILIZATION_ATTITUDE_PHI_IGAIN ABS STABILIZATION_ATTITUDE_THETA_IGAIN ABS STABILIZATION_ATTITUDE_PSI_IGAIN ABS STABILIZATION_ATTITUDE_PHI_DDGAIN ABS STABILIZATION_ATTITUDE_THETA_DDGAIN ABS STABILIZATION_ATTITUDE_PSI_DDGAIN EULERS_DIFF att_err att_ref_scaled ahrs.ltp_to_body_euler RATES_DIFF rate_err rate_ref_scaled ahrs.body_rate "
Paparazzi 112 stabilization_attitude_euler_int.c Non-Control 7fd01bcbe2 put generated headers in a seperate generated dir and specifically include generated/xxx.h " #include generated/airframe.h"
Paparazzi 113 stabilization_attitude_euler_int.c Non-Control 19199755fb cleanup a LOT of trailing whitespaces " #ifdef USE_HELI #endif"
Paparazzi 114 stabilization_attitude_euler_int.c Non-Control 08716bbd8d use double quotes for subsystems and firmware angle brackets only for external includes " #include firmwares/rotorcraft/stabilization.h #include subsystems/ahrs.h #include subsystems/radio_control.h"
Paparazzi 115 stabilization_attitude_euler_int.c Non-Control 5497b21831 start fixing booz radio control includes " #include "
Paparazzi 116 stabilization_attitude_euler_int.c Non-Control 79a927adfb moved ahrs to general subsystems dir " #include "
Paparazzi 117 stabilization_attitude_euler_int.c Non-Control 35830e36fd rename booz_stab_att_* to stab_att_* " STABILIZATION_ATTITUDE_READ_RC stab_att_sp_euler in_flight STABILIZATION_ATTITUDE_RESET_PSI_REF stab_att_sp_euler OFFSET_AND_ROUND stabilization_gains.dd.x * stab_att_ref_accel.p 5 OFFSET_AND_ROUND stabilization_gains.dd.y * stab_att_ref_accel.q 5 OFFSET_AND_ROUND stabilization_gains.dd.z * stab_att_ref_accel.r 5 OFFSET_AND_ROUND stab_att_ref_euler.phi REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND stab_att_ref_euler.theta REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND stab_att_ref_euler.psi REF_ANGLE_FRAC - INT32_ANGLE_FRAC OFFSET_AND_ROUND stab_att_ref_rate.p REF_RATE_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND stab_att_ref_rate.q REF_RATE_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND stab_att_ref_rate.r REF_RATE_FRAC - INT32_RATE_FRAC "
Paparazzi 118 stabilization_attitude_euler_int.c Control 6acb533a8f 10 bit resolution for heli actuators " #ifdef USE_HELI OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_ROLL +stabilization_att_ff_cmd COMMAND_ROLL 12 stabilization_cmd COMMAND_PITCH = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_PITCH +stabilization_att_ff_cmd COMMAND_PITCH 12 stabilization_cmd COMMAND_YAW = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_YAW +stabilization_att_ff_cmd COMMAND_YAW 12 #else stabilization_cmd COMMAND_ROLL = #endif"
Paparazzi 119 stabilization_attitude_euler_int.c Non-Control 963450f6a0 rename booz_stabilization to stabilization " * * Boston MA 02111-1307 USA. #include struct Int32AttitudeGains stabilization_gains struct Int32Eulers stabilization_att_sum_err int32_t stabilization_att_fb_cmd COMMANDS_NB int32_t stabilization_att_ff_cmd COMMANDS_NB void stabilization_attitude_init void stabilization_attitude_ref_init VECT3_ASSIGN stabilization_gains.p STABILIZATION_ATTITUDE_PHI_PGAIN STABILIZATION_ATTITUDE_THETA_PGAIN STABILIZATION_ATTITUDE_PSI_PGAIN VECT3_ASSIGN stabilization_gains.d STABILIZATION_ATTITUDE_PHI_DGAIN STABILIZATION_ATTITUDE_THETA_DGAIN STABILIZATION_ATTITUDE_PSI_DGAIN VECT3_ASSIGN stabilization_gains.i STABILIZATION_ATTITUDE_PHI_IGAIN STABILIZATION_ATTITUDE_THETA_IGAIN STABILIZATION_ATTITUDE_PSI_IGAIN VECT3_ASSIGN stabilization_gains.dd STABILIZATION_ATTITUDE_PHI_DDGAIN STABILIZATION_ATTITUDE_THETA_DDGAIN STABILIZATION_ATTITUDE_PSI_DDGAIN INT_EULERS_ZERO stabilization_att_sum_err void stabilization_attitude_read_rc bool_t in_flight STABILIZATION_ATTITUDE_READ_RC booz_stab_att_sp_euler in_flight void stabilization_attitude_enter void STABILIZATION_ATTITUDE_RESET_PSI_REF booz_stab_att_sp_euler INT_EULERS_ZERO stabilization_att_sum_err #define OFFSET_AND_ROUND _a _b _a + 1<< _b -1 >> _b void stabilization_attitude_run bool_t in_flight stabilization_attitude_ref_update stabilization_att_ff_cmd COMMAND_ROLL = OFFSET_AND_ROUND stabilization_gains.dd.x * booz_stab_att_ref_accel.p 5 stabilization_att_ff_cmd COMMAND_PITCH = OFFSET_AND_ROUND stabilization_gains.dd.y * booz_stab_att_ref_accel.q 5 stabilization_att_ff_cmd COMMAND_YAW = OFFSET_AND_ROUND stabilization_gains.dd.z * booz_stab_att_ref_accel.r 5 EULERS_ADD stabilization_att_sum_err att_err EULERS_BOUND_CUBE stabilization_att_sum_err -MAX_SUM_ERR MAX_SUM_ERR INT_EULERS_ZERO stabilization_att_sum_err stabilization_att_fb_cmd COMMAND_ROLL = stabilization_gains.p.x * att_err.phi + stabilization_gains.d.x * rate_err.p + OFFSET_AND_ROUND2 stabilization_gains.i.x * stabilization_att_sum_err.phi 10 stabilization_att_fb_cmd COMMAND_PITCH = stabilization_gains.p.y * att_err.theta + stabilization_gains.d.y * rate_err.q + OFFSET_AND_ROUND2 stabilization_gains.i.y * stabilization_att_sum_err.theta 10 stabilization_att_fb_cmd COMMAND_YAW = stabilization_gains.p.z * att_err.psi + stabilization_gains.d.z * rate_err.r + OFFSET_AND_ROUND2 stabilization_gains.i.z * stabilization_att_sum_err.psi 10 stabilization_cmd COMMAND_ROLL = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_ROLL +stabilization_att_ff_cmd COMMAND_ROLL 16 stabilization_cmd COMMAND_PITCH = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_PITCH +stabilization_att_ff_cmd COMMAND_PITCH 16 stabilization_cmd COMMAND_YAW = OFFSET_AND_ROUND stabilization_att_fb_cmd COMMAND_YAW +stabilization_att_ff_cmd COMMAND_YAW 16 "
Paparazzi 120 stabilization_attitude_euler_int.c Non-Control e68d6b372e moved booz stabilization to firmwares/rotorcraft still need to rename and adjust makefiles ""
Paparazzi 121 booz_stabilization_attitude_euler_int.c Non-Control 1cb5c50581 use full ahrs include path " #include "
Paparazzi 122 booz_stabilization_attitude_euler_int.c Non-Control c2a645a7ee rename booz_ahrs to ahrs fix makefiles " #include ahrs.h EULERS_DIFF att_err ahrs.ltp_to_body_euler att_ref_scaled RATES_DIFF rate_err ahrs.body_rate rate_ref_scaled "
Paparazzi 123 booz_stabilization_attitude_euler_int.c Non-Control 9c8a34fca5 Clean up booz_stabilization_attitude_quat_float.c by removing plenty of junk from it and pushing said bits down into ref or setpointgeneration or elsewhere entirely.Also move all of booz_stabilization gains into a single struct (potentially useful for gain scheduling or other hackery) " struct Int32AttitudeGains booz_stabilization_gains VECT3_ASSIGN booz_stabilization_gains.p VECT3_ASSIGN booz_stabilization_gains.d VECT3_ASSIGN booz_stabilization_gains.i VECT3_ASSIGN booz_stabilization_gains.dd OFFSET_AND_ROUND booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p 5 OFFSET_AND_ROUND booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q 5 OFFSET_AND_ROUND booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r 5 booz_stabilization_gains.p.x * att_err.phi + booz_stabilization_gains.d.x * rate_err.p + OFFSET_AND_ROUND2 booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi 10 booz_stabilization_gains.p.y * att_err.theta + booz_stabilization_gains.d.y * rate_err.q + OFFSET_AND_ROUND2 booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta 10 booz_stabilization_gains.p.z * att_err.psi + booz_stabilization_gains.d.z * rate_err.r + OFFSET_AND_ROUND2 booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi 10 "
Paparazzi 124 booz_stabilization_attitude_euler_int.c Control 703ba70552 Make ref_quat_float reference attitude generation not depend on in_flight mode again " booz_stabilization_attitude_ref_update "
Paparazzi 125 booz_stabilization_attitude_euler_int.c Control de7d066ebe Implement rate-mode pitch/roll sticks for ref_quat_float (instead of postition setpoints). Requires BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A/E and conditionally enabled by STICKS_PITCH_RATE / STICKS_ROLL_RATE #defines. Also make booz_stabilization_attitude_ref_update take in_flight as an argument and skip updating yaw reference acceleration when not in_flight. " booz_stabilization_attitude_ref_update in_flight "
Paparazzi 127 stabilization_rate.c Control 0c95b9e26e [rotorcraft] converted PI rate controller to floating pointcloses #1624 " #define MAX_SUM_ERR 40000 struct FloatRates stabilization_rate_sp struct FloatRates stabilization_rate_gain struct FloatRates stabilization_rate_igain struct FloatRates stabilization_rate_sum_err struct FloatRates stabilization_rate_fb_cmd FLOAT_RATES_ZERO stabilization_rate_sp FLOAT_RATES_ZERO stabilization_rate_sum_err stabilization_rate_sp.p = radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.q = radio_control.values RADIO_PITCH * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.r = radio_control.values RADIO_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ stabilization_rate_sp.r = - radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.q = radio_control.values RADIO_PITCH * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.p = radio_control.values RADIO_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ FLOAT_RATES_ZERO stabilization_rate_sum_err struct FloatRates _error struct FloatRates *body_rate = stateGetBodyRates_f struct FloatRates sum_err_increment RATES_SDIV sum_err_increment _error PERIODIC_FREQUENCY FLOAT_RATES_ZERO stabilization_rate_sum_err stabilization_rate_igain.p * stabilization_rate_sum_err.p stabilization_rate_igain.q * stabilization_rate_sum_err.q stabilization_rate_igain.r * stabilization_rate_sum_err.r stabilization_cmd COMMAND_ROLL = stabilization_rate_fb_cmd.p stabilization_cmd COMMAND_PITCH = stabilization_rate_fb_cmd.q stabilization_cmd COMMAND_YAW = stabilization_rate_fb_cmd.r "
Paparazzi 128 stabilization_rate.c Control d29c83aab9 remove scaling from rate feedback (#1601)Currently gains from rate control do not carry over to attitude stabilization because there is a factor 2 in the rate control and prescaler values (factor 3 for rate) in the attitude control. I searched through the airframes and there are not many that use rate control. I doubled all the gains and removed rate control for bebop as rate control over datalink is very difficult due to lag. The prescaler values should also be default 1 for stabilization quat_int as they already are for float_quat. However this will mean updating a large amount of airframes which will require some scripting. I might have time for this later. " stabilization_cmd COMMAND_ROLL = stabilization_rate_fb_cmd.p >> 12 stabilization_cmd COMMAND_PITCH = stabilization_rate_fb_cmd.q >> 12 stabilization_cmd COMMAND_YAW = stabilization_rate_fb_cmd.r >> 12 "
Paparazzi 129 stabilization_rate.c Non-Control 5c1e4260fe replace bool_t with C99 bool from stdbool.h " void stabilization_rate_run bool in_flight "
Paparazzi 130 stabilization_rate.c Control 799a9a7586 fix integrator scaling for rate control " struct Int32Rates sum_err_increment RATES_SDIV sum_err_increment _error 5 RATES_ADD stabilization_rate_sum_err sum_err_increment OFFSET_AND_ROUND2 stabilization_rate_igain.p * stabilization_rate_sum_err.p 6 OFFSET_AND_ROUND2 stabilization_rate_igain.q * stabilization_rate_sum_err.q 6 OFFSET_AND_ROUND2 stabilization_rate_igain.r * stabilization_rate_sum_err.r 6 "
Paparazzi 131 stabilization_rate.c Control 2a6b981b24 rate sp in degrees/s " stabilization_rate_sp.p = int32_t radio_control.values RADIO_ROLL * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.q = int32_t radio_control.values RADIO_PITCH * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.r = int32_t radio_control.values RADIO_YAW * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ stabilization_rate_sp.r = int32_t - radio_control.values RADIO_ROLL * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.q = int32_t radio_control.values RADIO_PITCH * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.p = int32_t radio_control.values RADIO_YAW * RATE_BFP_OF_REAL STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ "
Paparazzi 132 stabilization_rate.c Non-Control faf2206c32 [telemetry] register_periodic: use PPRZ_MSG_ID_x instead of DL_x " register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_RATE_LOOP send_rate "
Paparazzi 133 stabilization_rate.c Non-Control 83065af402 Change register_periodic_telemetry() to use msg id " register_periodic_telemetry DefaultPeriodic DL_RATE_LOOP send_rate "
Paparazzi 134 stabilization_rate.c Control bf06d09f29 removed everything related to rate reference " STABILIZATION_RATE_IGAIN_R stabilization_cmd COMMAND_ROLL = stabilization_rate_fb_cmd.p >> 11 stabilization_cmd COMMAND_PITCH = stabilization_rate_fb_cmd.q >> 11 stabilization_cmd COMMAND_YAW = stabilization_rate_fb_cmd.r >> 11 "
Paparazzi 135 stabilization_rate.c Control 528cfdab82 pleasant rate control " RATES_DIFF _error stabilization_rate_sp *body_rate stabilization_cmd COMMAND_ROLL = stabilization_rate_fb_cmd.p stabilization_cmd COMMAND_PITCH = stabilization_rate_fb_cmd.q stabilization_cmd COMMAND_YAW = stabilization_rate_fb_cmd.r "
Paparazzi 136 stabilization_rate.c Non-Control 603b40a513 [airborne] fix code style on (nearly) all files```find sw/airborne/ -regextype posix-extended -regex sw/airborne/.*(chibios-libopencm3|lpcusb|efsl|lpc21/include|lpc21/test/bootloader|subsystems/ahrs) -prune -o -name *.[ch] -exec ./fix_code_style.sh {} +```ignored ahrs (for now) to not create unnecessary conflicts for some pending changes.. " static void send_rate struct transport_tx *trans struct link_device *dev &stabilization_rate_sp.p &stabilization_rate_sp.q &stabilization_rate_sp.r &stabilization_rate_ref.p &stabilization_rate_ref.q &stabilization_rate_ref.r &stabilization_rate_refdot.p &stabilization_rate_refdot.q &stabilization_rate_refdot.r &stabilization_rate_sum_err.p &stabilization_rate_sum_err.q &stabilization_rate_sum_err.r &stabilization_rate_ff_cmd.p &stabilization_rate_ff_cmd.q &stabilization_rate_ff_cmd.r &stabilization_rate_fb_cmd.p &stabilization_rate_fb_cmd.q &stabilization_rate_fb_cmd.r &stabilization_cmd COMMAND_THRUST void stabilization_rate_init void void stabilization_rate_read_rc void if ROLL_RATE_DEADBAND_EXCEEDED else if PITCH_RATE_DEADBAND_EXCEEDED else if YAW_RATE_DEADBAND_EXCEEDED && !THROTTLE_STICK_DOWN else void stabilization_rate_read_rc_switched_sticks void if ROLL_RATE_DEADBAND_EXCEEDED stabilization_rate_sp.r = int32_t - radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ else if PITCH_RATE_DEADBAND_EXCEEDED else if YAW_RATE_DEADBAND_EXCEEDED && !THROTTLE_STICK_DOWN else INT_RATES_LSHIFT stabilization_rate_sp stabilization_rate_sp REF_FRAC - INT32_RATE_FRAC void stabilization_rate_enter void void stabilization_rate_run bool_t in_flight stabilization_rate_refdot.p >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC stabilization_rate_refdot.q >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC stabilization_rate_refdot.r >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC OFFSET_AND_ROUND stabilization_rate_ref.r REF_FRAC - INT32_RATE_FRAC struct Int32Rates *body_rate = stateGetBodyRates_i else OFFSET_AND_ROUND2 stabilization_rate_igain.p * stabilization_rate_sum_err.p 10 OFFSET_AND_ROUND2 stabilization_rate_igain.q * stabilization_rate_sum_err.q 10 OFFSET_AND_ROUND2 stabilization_rate_igain.r * stabilization_rate_sum_err.r 10 "
Paparazzi 137 stabilization_rate.c Non-Control 34b6ca3a26 [telemetry] rename device to link_device " static void send_rate struct transport_tx *trans struct link_device *dev "
Paparazzi 138 stabilization_rate.c Non-Control e5cdfa1ce1 [telemetry] convert macros in registered callback to functions " pprz_msg_send_RATE_LOOP trans dev AC_ID "
Paparazzi 139 stabilization_rate.c Non-Control c4b73552cd [telemetry] change register callback to pass arguments " static void send_rate struct transport_tx *trans struct device *dev "
Paparazzi 140 stabilization_rate.c Non-Control 0c64a410c0 [rotorcraft] reorg stabilization includes/inittowards not requiring any specfic stabilization controllers (none rate attitude) " #include firmwares/rotorcraft/stabilization/stabilization_rate.h"
Paparazzi 141 stabilization_rate.c Non-Control 96778c01f3 [rotorcraft] include and dox cleanup " #include generated/airframe.h"
Paparazzi 142 stabilization_rate.c Control 1ff8bb587f [rotorcraft] RC input: zero yaw command if throttle is zeroshould fix #736 " #include firmwares/rotorcraft/autopilot_rc_helpers.h if ROLL_RATE_DEADBAND_EXCEEDED if PITCH_RATE_DEADBAND_EXCEEDED if YAW_RATE_DEADBAND_EXCEEDED && !THROTTLE_STICK_DOWN if ROLL_RATE_DEADBAND_EXCEEDED if PITCH_RATE_DEADBAND_EXCEEDED if YAW_RATE_DEADBAND_EXCEEDED && !THROTTLE_STICK_DOWN "
Paparazzi 143 stabilization_rate.c Non-Control 4601197d36 [rotorcraft] error on negative gains " #if STABILIZATION_RATE_GAIN_P < 0 || \ STABILIZATION_RATE_GAIN_Q < 0 || \ STABILIZATION_RATE_GAIN_R < 0 || \ STABILIZATION_RATE_IGAIN_P < 0 || \ STABILIZATION_RATE_IGAIN_Q < 0 || \ STABILIZATION_RATE_IGAIN_R < 0 #error ALL control gains have to be positive!!!"
Paparazzi 144 stabilization_rate.c Non-Control eba60ba1fe [telemetry] make specific flag for periodic telemetry " #if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY"
Paparazzi 145 stabilization_rate.c Non-Control 2b3a1da82e Merge remote-tracking branch paparazzi/master into telemetryConflicts:sw/airborne/firmwares/rotorcraft/autopilot.c ""
Paparazzi 146 stabilization_rate.c Non-Control 116faaeda6 [telemetry] protect telemetry code with #if DOWNLINK- also get rid of AP_DOWNLINK and FBW_DOWNLINK- still needs to make it work again (for the only remaining classix in the world...) " #if DOWNLINK #include subsystems/datalink/telemetry.h #endif #if DOWNLINK #endif"
Paparazzi 147 stabilization_rate.c Non-Control 1931ec6628 cleanup some trailing whitespace ""
Paparazzi 148 stabilization_rate.c Control 8377d60184 [telemetry] adapt rotorcraft to new telemetry system " #include subsystems/datalink/downlink.h #include generated/periodic_telemetry.h static void send_rate void DOWNLINK_SEND_RATE_LOOP DefaultChannel DefaultDevice &stabilization_rate_sp.p &stabilization_rate_sp.q &stabilization_rate_sp.r &stabilization_rate_ref.p &stabilization_rate_ref.q &stabilization_rate_ref.r &stabilization_rate_refdot.p &stabilization_rate_refdot.q &stabilization_rate_refdot.r &stabilization_rate_sum_err.p &stabilization_rate_sum_err.q &stabilization_rate_sum_err.r &stabilization_rate_ff_cmd.p &stabilization_rate_ff_cmd.q &stabilization_rate_ff_cmd.r &stabilization_rate_fb_cmd.p &stabilization_rate_fb_cmd.q &stabilization_rate_fb_cmd.r &stabilization_cmd COMMAND_THRUST register_periodic_telemetry DefaultPeriodic RATE_LOOP send_rate "
Paparazzi 149 stabilization_rate.c Control 3710206de3 Added possibility to switch sticks " void stabilization_rate_read_rc_switched_sticks void if ROLL_RATE_DEADBAND_EXCEEDED stabilization_rate_sp.r = int32_t -radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ else stabilization_rate_sp.r = 0 if PITCH_RATE_DEADBAND_EXCEEDED stabilization_rate_sp.q = int32_t radio_control.values RADIO_PITCH * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ else stabilization_rate_sp.q = 0 if YAW_RATE_DEADBAND_EXCEEDED stabilization_rate_sp.p = int32_t radio_control.values RADIO_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ else stabilization_rate_sp.p = 0 INT_RATES_LSHIFT stabilization_rate_sp stabilization_rate_sp REF_FRAC - INT32_RATE_FRAC "
Paparazzi 150 stabilization_rate.c Non-Control 5450ba7774 [stab rate] convert stabilization in rate mode to new state interface " #include state.h struct Int32Rates* body_rate = stateGetBodyRates_i RATES_DIFF _error _ref_scaled *body_rate "
Paparazzi 151 stabilization_rate.c Control 313bde5157 rotorcraft stabilization: updated gain scales to get roughly the same gains as before the change of commands* attitude int_quat: multiply old gain scale by 48 (9600/200)* attitude int euler: changed CMD_SHIFT from 16 to 11 so a multiply of 32 to be a bit more conservative for a start* rate: changed the command shift by 5 so also a conservative 32 " RATES_EWMULT_RSHIFT stabilization_rate_ff_cmd stabilization_rate_ddgain stabilization_rate_refdot 9 stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11 stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11 stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 11 "
Paparazzi 152 stabilization_rate.c Non-Control a24ae2d31c Merge branch 4.0_beta into dev ""
Paparazzi 153 stabilization_rate.c Non-Control c840489326 Merge branch dev into 4.0_beta* updated tests* set quaternion setpoint in hover and attitude nav ""
Paparazzi 154 stabilization_rate.c Control 1905e350c4 rotorcraft stabilization: bound stabilization_cmd to MAX_PPRZ " BoundAbs stabilization_cmd COMMAND_ROLL MAX_PPRZ BoundAbs stabilization_cmd COMMAND_PITCH MAX_PPRZ BoundAbs stabilization_cmd COMMAND_YAW MAX_PPRZ "
Paparazzi 155 stabilization_rate.c Non-Control fe7c72c741 rotorcraft guidance and stabilization: only documentation and cosmetics " "
Paparazzi 156 stabilization_rate.c Control 532c810c2b CAUTION changed roll and yaw input signs from rc to adhere to standard aerospace convention directions " stabilization_rate_sp.p = int32_t radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.r = int32_t radio_control.values RADIO_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ "
Paparazzi 157 stabilization_rate.c Control df76dc8fbe rotorcraft gains: do not use ABS anymore warn instead if gains are negative " #if STABILIZATION_RATE_GAIN_P < 0 || \ STABILIZATION_RATE_GAIN_Q < 0 || \ STABILIZATION_RATE_GAIN_R < 0 #warning ALL control gains are now positive!!! #endif #else #if STABILIZATION_RATE_IGAIN_P < 0 #warning ALL control gains are now positive!!! #endif #else #if STABILIZATION_RATE_IGAIN_Q < 0 #warning ALL control gains are now positive!!! #endif #else #if STABILIZATION_RATE_IGAIN_R < 0 #warning ALL control gains are now positive!!! #endif STABILIZATION_RATE_GAIN_P STABILIZATION_RATE_GAIN_Q STABILIZATION_RATE_GAIN_R STABILIZATION_RATE_IGAIN_P STABILIZATION_RATE_IGAIN_Q STABILIZATION_RATE_IGAIN_R STABILIZATION_RATE_DDGAIN_P STABILIZATION_RATE_DDGAIN_Q STABILIZATION_RATE_DDGAIN_R "
Paparazzi 158 stabilization_rate.c Control 070eb4c064 changed stabilization_rate gains to positive " ABS STABILIZATION_RATE_GAIN_P ABS STABILIZATION_RATE_GAIN_Q ABS STABILIZATION_RATE_GAIN_R ABS STABILIZATION_RATE_IGAIN_P ABS STABILIZATION_RATE_IGAIN_Q ABS STABILIZATION_RATE_IGAIN_R ABS STABILIZATION_RATE_DDGAIN_P ABS STABILIZATION_RATE_DDGAIN_Q ABS STABILIZATION_RATE_DDGAIN_R RATES_DIFF _error _ref_scaled ahrs.body_rate "
Paparazzi 159 stabilization_rate.c Control 0d6a13e000 fix rate mode " INT_RATES_LSHIFT stabilization_rate_sp stabilization_rate_sp REF_FRAC - INT32_RATE_FRAC "
Paparazzi 160 stabilization_rate.c Non-Control 7fd01bcbe2 put generated headers in a seperate generated dir and specifically include generated/xxx.h " #include generated/airframe.h"
Paparazzi 161 stabilization_rate.c Non-Control 08716bbd8d use double quotes for subsystems and firmware angle brackets only for external includes " #include firmwares/rotorcraft/stabilization.h #include subsystems/ahrs.h #include subsystems/imu.h #include subsystems/radio_control.h"
Paparazzi 162 stabilization_rate.c Non-Control a8c1b523cd update rotorcraft to use RADIO_THROTTLE instead of RADIO_CONTROL_THROTTLE etc. " radio_control.values RADIO_ROLL > STABILIZATION_RATE_DEADBAND_P || \ radio_control.values RADIO_ROLL < -STABILIZATION_RATE_DEADBAND_P radio_control.values RADIO_PITCH > STABILIZATION_RATE_DEADBAND_Q || \ radio_control.values RADIO_PITCH < -STABILIZATION_RATE_DEADBAND_Q radio_control.values RADIO_YAW > STABILIZATION_RATE_DEADBAND_R || \ radio_control.values RADIO_YAW < -STABILIZATION_RATE_DEADBAND_R stabilization_rate_sp.p = int32_t -radio_control.values RADIO_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.q = int32_t radio_control.values RADIO_PITCH * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.r = int32_t -radio_control.values RADIO_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ "
Paparazzi 163 stabilization_rate.c Non-Control 5497b21831 start fixing booz radio control includes " #include "
Paparazzi 164 stabilization_rate.c Non-Control 79a927adfb moved ahrs to general subsystems dir " #include "
Paparazzi 165 stabilization_rate.c Non-Control 38c909d66a correct includes after moving imu " #include "
Paparazzi 166 stabilization_rate.c Non-Control 963450f6a0 rename booz_stabilization to stabilization " #include #ifndef STABILIZATION_RATE_DDGAIN_P #define STABILIZATION_RATE_DDGAIN_P 0 #ifndef STABILIZATION_RATE_DDGAIN_Q #define STABILIZATION_RATE_DDGAIN_Q 0 #ifndef STABILIZATION_RATE_DDGAIN_R #define STABILIZATION_RATE_DDGAIN_R 0 #ifndef STABILIZATION_RATE_IGAIN_P #define STABILIZATION_RATE_IGAIN_P 0 #ifndef STABILIZATION_RATE_IGAIN_Q #define STABILIZATION_RATE_IGAIN_Q 0 #ifndef STABILIZATION_RATE_IGAIN_R #define STABILIZATION_RATE_IGAIN_R 0 #ifndef STABILIZATION_RATE_REF_TAU #define STABILIZATION_RATE_REF_TAU 4 struct Int32Rates stabilization_rate_sp struct Int32Rates stabilization_rate_gain struct Int32Rates stabilization_rate_igain struct Int32Rates stabilization_rate_ddgain struct Int32Rates stabilization_rate_ref struct Int32Rates stabilization_rate_refdot struct Int32Rates stabilization_rate_sum_err struct Int32Rates stabilization_rate_fb_cmd struct Int32Rates stabilization_rate_ff_cmd #ifndef STABILIZATION_RATE_DEADBAND_P #define STABILIZATION_RATE_DEADBAND_P 0 #ifndef STABILIZATION_RATE_DEADBAND_Q #define STABILIZATION_RATE_DEADBAND_Q 0 #ifndef STABILIZATION_RATE_DEADBAND_R #define STABILIZATION_RATE_DEADBAND_R 200 radio_control.values RADIO_CONTROL_ROLL > STABILIZATION_RATE_DEADBAND_P || \ radio_control.values RADIO_CONTROL_ROLL < -STABILIZATION_RATE_DEADBAND_P radio_control.values RADIO_CONTROL_PITCH > STABILIZATION_RATE_DEADBAND_Q || \ radio_control.values RADIO_CONTROL_PITCH < -STABILIZATION_RATE_DEADBAND_Q radio_control.values RADIO_CONTROL_YAW > STABILIZATION_RATE_DEADBAND_R || \ radio_control.values RADIO_CONTROL_YAW < -STABILIZATION_RATE_DEADBAND_R void stabilization_rate_init void INT_RATES_ZERO stabilization_rate_sp RATES_ASSIGN stabilization_rate_gain STABILIZATION_RATE_GAIN_P STABILIZATION_RATE_GAIN_Q STABILIZATION_RATE_GAIN_R RATES_ASSIGN stabilization_rate_igain STABILIZATION_RATE_IGAIN_P STABILIZATION_RATE_IGAIN_Q STABILIZATION_RATE_IGAIN_R RATES_ASSIGN stabilization_rate_ddgain STABILIZATION_RATE_DDGAIN_P STABILIZATION_RATE_DDGAIN_Q STABILIZATION_RATE_DDGAIN_R INT_RATES_ZERO stabilization_rate_ref INT_RATES_ZERO stabilization_rate_refdot INT_RATES_ZERO stabilization_rate_sum_err void stabilization_rate_read_rc void stabilization_rate_sp.p = int32_t -radio_control.values RADIO_CONTROL_ROLL * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ stabilization_rate_sp.p = 0 stabilization_rate_sp.q = int32_t radio_control.values RADIO_CONTROL_PITCH * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ stabilization_rate_sp.q = 0 stabilization_rate_sp.r = int32_t -radio_control.values RADIO_CONTROL_YAW * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ stabilization_rate_sp.r = 0 void stabilization_rate_enter void RATES_COPY stabilization_rate_ref stabilization_rate_sp INT_RATES_ZERO stabilization_rate_sum_err void stabilization_rate_run bool_t in_flight RATES_DIFF _r stabilization_rate_sp stabilization_rate_ref RATES_SDIV stabilization_rate_refdot _r STABILIZATION_RATE_REF_TAU stabilization_rate_refdot.p >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC stabilization_rate_refdot.q >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC stabilization_rate_refdot.r >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC RATES_ADD stabilization_rate_ref _delta_ref RATES_EWMULT_RSHIFT stabilization_rate_ff_cmd stabilization_rate_ddgain stabilization_rate_refdot 14 OFFSET_AND_ROUND stabilization_rate_ref.p REF_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND stabilization_rate_ref.q REF_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND stabilization_rate_ref.r REF_FRAC - INT32_RATE_FRAC RATES_ADD stabilization_rate_sum_err _error RATES_BOUND_CUBE stabilization_rate_sum_err -MAX_SUM_ERR MAX_SUM_ERR INT_RATES_ZERO stabilization_rate_sum_err stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p + OFFSET_AND_ROUND2 stabilization_rate_igain.p * stabilization_rate_sum_err.p 10 stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2 stabilization_rate_igain.q * stabilization_rate_sum_err.q 10 stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2 stabilization_rate_igain.r * stabilization_rate_sum_err.r 10 stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 16 stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 16 stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 16 stabilization_cmd COMMAND_ROLL = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p stabilization_cmd COMMAND_PITCH = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q stabilization_cmd COMMAND_YAW = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r "
Paparazzi 167 booz_stabilization_rate.c Non-Control e68d6b372e moved booz stabilization to firmwares/rotorcraft still need to rename and adjust makefiles ""
Paparazzi 168 booz_stabilization_rate.c Non-Control 1cb5c50581 use full ahrs include path " #include "
Paparazzi 169 booz_stabilization_rate.c Non-Control 8e7e499bb6 use full path to imu includes " #include "
Paparazzi 170 booz_stabilization_rate.c Non-Control c2a645a7ee rename booz_ahrs to ahrs fix makefiles " #include ahrs.h RATES_DIFF _error ahrs.body_rate _ref_scaled "
Paparazzi 171 booz_stabilization_rate.c Non-Control 28118816bd rename booz_imu to imu fix imu makefiles " #include imu.h"
Paparazzi 172 booz_stabilization_rate.c Control 7749ca640e change scaling of feedforward gains in rate mode gains effectively four times higher now " RATES_EWMULT_RSHIFT booz_stabilization_rate_ff_cmd booz_stabilization_rate_ddgain booz_stabilization_rate_refdot 14 "
Paparazzi 173 booz_stabilization_rate.c Control 6bef819be9 stabilization_rate: compute feedback error between reference and actual rate instead of stick setpoint and actual rate " const struct Int32Rates _ref_scaled = OFFSET_AND_ROUND booz_stabilization_rate_ref.p REF_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND booz_stabilization_rate_ref.q REF_FRAC - INT32_RATE_FRAC OFFSET_AND_ROUND booz_stabilization_rate_ref.r REF_FRAC - INT32_RATE_FRAC RATES_DIFF _error booz_ahrs.body_rate _ref_scaled "
Paparazzi 174 booz_stabilization_rate.c Control 78b9cd3f12 add deadbands to rate mode " #ifndef BOOZ_STABILIZATION_RATE_REF_TAU #define BOOZ_STABILIZATION_RATE_REF_TAU 4 #ifndef BOOZ_STABILIZATION_RATE_DEADBAND_P #define BOOZ_STABILIZATION_RATE_DEADBAND_P 0 #endif #ifndef BOOZ_STABILIZATION_RATE_DEADBAND_Q #define BOOZ_STABILIZATION_RATE_DEADBAND_Q 0 #endif #ifndef BOOZ_STABILIZATION_RATE_DEADBAND_R #define BOOZ_STABILIZATION_RATE_DEADBAND_R 200 #endif #define ROLL_RATE_DEADBAND_EXCEEDED \ radio_control.values RADIO_CONTROL_ROLL > BOOZ_STABILIZATION_RATE_DEADBAND_P || \ radio_control.values RADIO_CONTROL_ROLL < -BOOZ_STABILIZATION_RATE_DEADBAND_P #define PITCH_RATE_DEADBAND_EXCEEDED \ radio_control.values RADIO_CONTROL_PITCH > BOOZ_STABILIZATION_RATE_DEADBAND_Q || \ radio_control.values RADIO_CONTROL_PITCH < -BOOZ_STABILIZATION_RATE_DEADBAND_Q #define YAW_RATE_DEADBAND_EXCEEDED \ radio_control.values RADIO_CONTROL_YAW > BOOZ_STABILIZATION_RATE_DEADBAND_R || \ radio_control.values RADIO_CONTROL_YAW < -BOOZ_STABILIZATION_RATE_DEADBAND_R if ROLL_RATE_DEADBAND_EXCEEDED booz_stabilization_rate_sp.p = int32_t -radio_control.values RADIO_CONTROL_ROLL * BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ else booz_stabilization_rate_sp.p = 0 if PITCH_RATE_DEADBAND_EXCEEDED booz_stabilization_rate_sp.q = int32_t radio_control.values RADIO_CONTROL_PITCH * BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ else booz_stabilization_rate_sp.q = 0 if YAW_RATE_DEADBAND_EXCEEDED booz_stabilization_rate_sp.r = int32_t -radio_control.values RADIO_CONTROL_YAW * BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ else booz_stabilization_rate_sp.r = 0 RATES_SDIV booz_stabilization_rate_refdot _r BOOZ_STABILIZATION_RATE_REF_TAU "
Paparazzi 175 booz_stabilization_rate.c Control c5eeae93ea disable integrator term in rate by default " #define BOOZ_STABILIZATION_RATE_IGAIN_P 0 #define BOOZ_STABILIZATION_RATE_IGAIN_Q 0 #define BOOZ_STABILIZATION_RATE_IGAIN_R 0"
Paparazzi 176 booz_stabilization_rate.c Control d6b0c1d67c add integral part to rate stabilization " #define MAX_SUM_ERR 4000000 #ifndef BOOZ_STABILIZATION_RATE_IGAIN_P #define BOOZ_STABILIZATION_RATE_IGAIN_P -64 #endif #ifndef BOOZ_STABILIZATION_RATE_IGAIN_Q #define BOOZ_STABILIZATION_RATE_IGAIN_Q -64 #endif #ifndef BOOZ_STABILIZATION_RATE_IGAIN_R #define BOOZ_STABILIZATION_RATE_IGAIN_R -32 #endif #define BOOZ_STABILIZATION_RATE_TAU 4 #define OFFSET_AND_ROUND _a _b _a + 1<< _b -1 >> _b #define OFFSET_AND_ROUND2 _a _b _a + 1<< _b -1 - _a <0?1:0 >> _b struct Int32Rates booz_stabilization_rate_igain struct Int32Rates booz_stabilization_rate_sum_err RATES_ASSIGN booz_stabilization_rate_igain BOOZ_STABILIZATION_RATE_IGAIN_P BOOZ_STABILIZATION_RATE_IGAIN_Q BOOZ_STABILIZATION_RATE_IGAIN_R INT_RATES_ZERO booz_stabilization_rate_sum_err INT_RATES_ZERO booz_stabilization_rate_sum_err void booz_stabilization_rate_run bool_t in_flight if in_flight RATES_ADD booz_stabilization_rate_sum_err _error RATES_BOUND_CUBE booz_stabilization_rate_sum_err -MAX_SUM_ERR MAX_SUM_ERR else INT_RATES_ZERO booz_stabilization_rate_sum_err booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_gain.p * _error.p + OFFSET_AND_ROUND2 booz_stabilization_rate_igain.p * booz_stabilization_rate_sum_err.p 10 booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2 booz_stabilization_rate_igain.q * booz_stabilization_rate_sum_err.q 10 booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2 booz_stabilization_rate_igain.r * booz_stabilization_rate_sum_err.r 10 booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_fb_cmd.p >> 16 booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_fb_cmd.q >> 16 booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_fb_cmd.r >> 16 "
Paparazzi 177 booz_stabilization_rate.c Control a083f4a7e7 add feedforward to rate stabilization " * Copyright C 2010 Felix Ruess #define F_UPDATE_RES 9 #define REF_DOT_FRAC 11 #define REF_FRAC 16 #ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P #define BOOZ_STABILIZATION_RATE_DDGAIN_P 0 #endif #ifndef BOOZ_STABILIZATION_RATE_DDGAIN_Q #define BOOZ_STABILIZATION_RATE_DDGAIN_Q 0 #endif #ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R #define BOOZ_STABILIZATION_RATE_DDGAIN_R 0 #endif #ifndef BOOZ_STABILIZATION_RATE_TAU #define BOOZ_STABILIZATION_RATE_TAU 1 #endif struct Int32Rates booz_stabilization_rate_ddgain struct Int32Rates booz_stabilization_rate_ref struct Int32Rates booz_stabilization_rate_refdot struct Int32Rates booz_stabilization_rate_fb_cmd struct Int32Rates booz_stabilization_rate_ff_cmd BOOZ_STABILIZATION_RATE_GAIN_P BOOZ_STABILIZATION_RATE_GAIN_Q BOOZ_STABILIZATION_RATE_GAIN_R RATES_ASSIGN booz_stabilization_rate_ddgain BOOZ_STABILIZATION_RATE_DDGAIN_P BOOZ_STABILIZATION_RATE_DDGAIN_Q BOOZ_STABILIZATION_RATE_DDGAIN_R INT_RATES_ZERO booz_stabilization_rate_ref INT_RATES_ZERO booz_stabilization_rate_refdot void booz_stabilization_rate_enter void RATES_COPY booz_stabilization_rate_ref booz_stabilization_rate_sp struct Int32Rates _r RATES_DIFF _r booz_stabilization_rate_sp booz_stabilization_rate_ref RATES_SDIV booz_stabilization_rate_refdot _r BOOZ_STABILIZATION_RATE_TAU const struct Int32Rates _delta_ref = booz_stabilization_rate_refdot.p >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC booz_stabilization_rate_refdot.q >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC booz_stabilization_rate_refdot.r >> F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC RATES_ADD booz_stabilization_rate_ref _delta_ref RATES_EWMULT_RSHIFT booz_stabilization_rate_ff_cmd booz_stabilization_rate_ddgain booz_stabilization_rate_refdot 16 RATES_EWMULT_RSHIFT booz_stabilization_rate_fb_cmd booz_stabilization_rate_gain _error 16 booz_stabilization_cmd COMMAND_ROLL = booz_stabilization_rate_ff_cmd.p + booz_stabilization_rate_fb_cmd.p booz_stabilization_cmd COMMAND_PITCH = booz_stabilization_rate_ff_cmd.q + booz_stabilization_rate_fb_cmd.q booz_stabilization_cmd COMMAND_YAW = booz_stabilization_rate_ff_cmd.r + booz_stabilization_rate_fb_cmd.r "
Paparazzi 178 booz_stabilization_rate.c Non-Control dc97c7cc48 remove rate measurement var ""
Paparazzi 179 booz_stabilization_rate.c Control 39079ecef6 use body rate from ahrs in rate mode " * * Boston MA 02111-1307 USA. #include booz_ahrs.h RATES_DIFF _error booz_ahrs.body_rate booz_stabilization_rate_sp "
Paparazzi 180 booz_stabilization_rate.c Control c14bf86b83 init measurement with zero in rate mode " INT_RATES_ZERO booz_stabilization_rate_measure "
Paparazzi 181 booz_stabilization_rate.c Non-Control 24ad57ca46 moving files " #include booz_stabilization.h struct Int32Rates booz_stabilization_rate_measure struct Int32Rates booz_stabilization_rate_sp struct Int32Rates booz_stabilization_rate_gain void booz_stabilization_rate_init void INT_RATES_ZERO booz_stabilization_rate_sp RATES_ASSIGN booz_stabilization_rate_gain void booz_stabilization_rate_read_rc void RATES_ASSIGN booz_stabilization_rate_sp void booz_stabilization_rate_run void RATES_ADD booz_stabilization_rate_measure booz_imu.gyro RATES_SUB booz_stabilization_rate_measure booz_ahrs_aligner.lp_gyro RATES_SDIV booz_stabilization_rate_measure booz_stabilization_rate_measure 2 RATES_DIFF _error booz_stabilization_rate_measure booz_stabilization_rate_sp RATES_EWMULT_RSHIFT _cmd _error booz_stabilization_rate_gain 16 booz_stabilization_cmd COMMAND_ROLL = _cmd.p booz_stabilization_cmd COMMAND_PITCH = _cmd.q booz_stabilization_cmd COMMAND_YAW = _cmd.r "
Paparazzi 182 booz2_stabilization_rate.c Non-Control 8d8bc2f3ba moving files around " #include ahrs/booz_ahrs_aligner.h"
Paparazzi 183 booz2_stabilization_rate.c Non-Control fa2d6026e6 big files move/renamereplaced CONFIG with BOARD_CONFIGremoved booz_geometry_xxxx (now replaced by math/pprz_trig_int and math/pprz_algebra_xxx )moved booz/arm7 to booz/arch/lpc21 and booz/stm32 to booz/arch/stm32 " #include booz_imu.h"
Paparazzi 184 booz2_stabilization_rate.c Control 033d6334b1 switched to new radio_control code that supports both classic ppm scheme and newer serial frames from 2.4GHz spektrum transmitters. created two makefiles to select between modes " #include booz_radio_control.h int32_t -radio_control.values RADIO_CONTROL_ROLL * BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ int32_t radio_control.values RADIO_CONTROL_PITCH * BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ int32_t -radio_control.values RADIO_CONTROL_YAW * BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ "
Paparazzi 185 booz2_stabilization_rate.c Non-Control 36e44fdb8d fixing licence headers thank you Piotr " "
Paparazzi 186 booz2_stabilization_rate.c Non-Control 1d7290e7ce *** empty log message *** " #include booz_ahrs_aligner.h struct Int32Rates booz2_stabilization_rate_measure struct Int32Rates booz2_stabilization_rate_sp struct Int32Rates booz2_stabilization_rate_gain INT_RATES_ZERO booz2_stabilization_rate_sp RATES_ASSIGN booz2_stabilization_rate_gain BOOZ_STABILIZATION_RATE_GAIN_P BOOZ_STABILIZATION_RATE_GAIN_Q BOOZ_STABILIZATION_RATE_GAIN_R RATES_ASSIGN booz2_stabilization_rate_sp int32_t -rc_values RADIO_ROLL * BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ int32_t rc_values RADIO_PITCH * BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ int32_t -rc_values RADIO_YAW * BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ RATES_ADD booz2_stabilization_rate_measure booz_imu.gyro if booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED RATES_SUB booz2_stabilization_rate_measure booz_ahrs_aligner.lp_gyro RATES_SDIV booz2_stabilization_rate_measure booz2_stabilization_rate_measure 2 struct Int32Rates _error RATES_DIFF _error booz2_stabilization_rate_measure booz2_stabilization_rate_sp struct Int32Rates _cmd RATES_EWMULT_RSHIFT _cmd _error booz2_stabilization_rate_gain 16 booz2_stabilization_cmd COMMAND_ROLL = _cmd.p booz2_stabilization_cmd COMMAND_PITCH = _cmd.q booz2_stabilization_cmd COMMAND_YAW = _cmd.r "
Paparazzi 188 baro_board.c Non-Control 995be46092 fix some more implicit-fallthrough warnings ""
Paparazzi 189 baro_board.c Non-Control 8edab84dd4 replace TRUE/FALSE by stdbool true/false " log_apogee_baro_started = false apogee_baro.data_available = false apogee_baro.data_available = false log_apogee_baro_started = true navdata.baro_available = false baro_bmp085.data_available = false baro_board.running = false baro_board.running = true baro_bmp085.data_available = false baro_bmp085.data_available = false mcp355x_data_available = false baro_bmp085.data_available = false BARO_ABS_ADS.data_available = false "
Paparazzi 190 baro_board.c Non-Control 5c1e4260fe replace bool_t with C99 bool from stdbool.h " bool log_apogee_baro_started bool running static bool baro_eoc void static bool baro_eoc void "
Paparazzi 191 baro_board.c Non-Control 30df14eb05 [abi] allow to pass variables by valuebefore the generated ABI callbacks always had a signature with `const type *var` where type was e.g float.Now the generated signature is simply `type var`.To pass const pointers again set it accordingly in abi.xml e.g. type= const float * instead of type= float " AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgTEMPERATURE BARO_BOARD_SENDER_ID temp AbiSendMsgTEMPERATURE BARO_BOARD_SENDER_ID temp_deg AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgBARO_DIFF BARO_BOARD_SENDER_ID diff AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgTEMPERATURE BARO_BOARD_SENDER_ID temp AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure AbiSendMsgTEMPERATURE BARO_BOARD_SENDER_ID temp AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID pressure "
Paparazzi 192 baro_board.c Non-Control 603b40a513 [airborne] fix code style on (nearly) all files```find sw/airborne/ -regextype posix-extended -regex sw/airborne/.*(chibios-libopencm3|lpcusb|efsl|lpc21/include|lpc21/test/bootloader|subsystems/ahrs) -prune -o -name *.[ch] -exec ./fix_code_style.sh {} +```ignored ahrs (for now) to not create unnecessary conflicts for some pending changes.. " void baro_init void void baro_periodic void void apogee_baro_event void float pressure = float apogee_baro.pressure / 1 << 2 void baro_init void int32_t b3 = int32_t baro_calibration.ac1 * 4 + x3 << BMP180_OSS + 2 / 4 uint32_t b4 = int32_t baro_calibration.ac4 * uint32_t x3 + 32768L >> 15 void baro_init void void baro_periodic void baro_board.absolute = baro_board.buf.sum / baro_board.buf.av_nb_sample baro_board.value_filtered = 3 * baro_board.value_filtered + baro_board.absolute / 4 else float pressure = 101325.0 - BOOZ_BARO_SENS * BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute void baro_board_calibrate void if baro_board.value_filtered == 0 && baro_board.offset > 15 else else void baro_init void void baro_periodic void else void bmp_baro_event void RunOnceEvery 10 LED_TOGGLE BARO_LED void baro_init void void baro_periodic void if !i2c_idle &i2c2 return case LBS_UNINITIALIZED: baro_board_send_reset baro_board.status = LBS_RESETED break case LBS_RESETED: baro_board_send_config_abs baro_board.status = LBS_INITIALIZING_ABS break case LBS_INITIALIZING_ABS: baro_board_set_current_register BARO_ABS_ADDR 0x00 baro_board.status = LBS_INITIALIZING_ABS_1 break case LBS_INITIALIZING_ABS_1: baro_board_send_config_diff baro_board.status = LBS_INITIALIZING_DIFF break case LBS_INITIALIZING_DIFF: baro_board_set_current_register BARO_DIFF_ADDR 0x00 baro_board.status = LBS_INITIALIZING_DIFF_1 break case LBS_INITIALIZING_DIFF_1: baro_board.running = TRUE case LBS_READ_DIFF: baro_board_read_from_current_register BARO_ABS_ADDR baro_board.status = LBS_READING_ABS break case LBS_READ_ABS: baro_board_read_from_current_register BARO_DIFF_ADDR baro_board.status = LBS_READING_DIFF break default: break else void lisa_l_baro_event void int16_t tmp = baro_trans.buf 0 << 8 | baro_trans.buf 1 float pressure = LISA_L_BARO_SENS * float tmp else if baro_board.status == LBS_READING_DIFF && baro_trans.status != I2CTransPending int16_t tmp = baro_trans.buf 0 << 8 | baro_trans.buf 1 float diff = LISA_L_DIFF_SENS * float tmp static inline void baro_board_send_config_abs void INFO Using High LisaL Baro Gain: Do not use below 1000hPa INFO Using Low LisaL Baro Gain capable of measuring below 1000hPa or more static inline void baro_board_send_config_diff void static inline void baro_board_send_reset void i2c_submit &i2c2 &baro_trans static inline void baro_board_write_to_register uint8_t baro_addr uint8_t reg_addr uint8_t val_msb uint8_t val_lsb i2c_submit &i2c2 &baro_trans static inline void baro_board_read_from_register uint8_t baro_addr uint8_t reg_addr i2c_submit &i2c2 &baro_trans static inline void baro_board_set_current_register uint8_t baro_addr uint8_t reg_addr i2c_submit &i2c2 &baro_trans static inline void baro_board_read_from_current_register uint8_t baro_addr i2c_submit &i2c2 &baro_trans void baro_init void GPIO_CNF_INPUT_PULL_UPDOWN GPIO0 void baro_periodic void else RunOnceEvery 10 LED_TOGGLE BARO_LED void baro_init void void baro_periodic void RunOnceEvery 4 mcp355x_read void navgo_baro_event void float pressure = NAVGO_BARO_SENS * mcp355x_data + NAVGO_BARO_OFFSET void baro_init void void baro_periodic void else void baro_event void RunOnceEvery 10 LED_TOGGLE BARO_LED void baro_init void void baro_periodic void void umarim_baro_event void float pressure = UMARIM_BARO_SENS * Ads1114GetValue BARO_ABS_ADS "
Paparazzi 193 baro_board.c Non-Control 78ede475d4 [abi] define ABI x_SENDER_IDs in abi_sender_ids.hSame BARO_BOARD_SENDER_ID for all baro_board implementations.Since there can only be one baro_board at the same time andthis provides a good default for INS_BARO_ID.The abi_sender_ids.h can be considered a temporary solution we might come up with a different scheme later... " AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_DIFF BARO_BOARD_SENDER_ID &diff AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure AbiSendMsgBARO_ABS BARO_BOARD_SENDER_ID &pressure "
Paparazzi 194 baro_board.c Control 815f7a9a0f [baro] convert remaining baro_board to ABI " AbiSendMsgBARO_ABS APOGEE_BARO_SENDER_ID &pressure #include subsystems/abi.h #ifndef ARDRONE2_BARO_SENS #define ARDRONE2_BARO_SENS 1.0 #endif #ifndef ARDRONE2_BARO_SENDER_ID #define ARDRONE2_BARO_SENDER_ID 13 #endif void baro_init void void baro_periodic void void ardrone_baro_event void if navdata_baro_available if baro_calibrated baro_apply_calibration_temp navdata->temperature_pressure float pressure = ARDRONE2_BARO_SENS* float baro_apply_calibration navdata->pressure AbiSendMsgBARO_ABS ARDRONE2_BARO_SENDER_ID &pressure navdata_baro_available = FALSE #include std.h #include mcu_periph/i2c.h #include subsystems/abi.h #include led.h enum LisaBaroStatus LBS_UNINITIALIZED LBS_RESETED LBS_INITIALIZING_ABS LBS_INITIALIZING_ABS_1 LBS_INITIALIZING_DIFF LBS_INITIALIZING_DIFF_1 LBS_IDLE LBS_READING_ABS LBS_READ_ABS LBS_READING_DIFF LBS_READ_DIFF struct BaroBoard enum LisaBaroStatus status bool_t running static inline void baro_board_send_reset void static inline void baro_board_send_config_abs void static inline void baro_board_send_config_diff void #ifndef LISA_L_BARO_SENS #define LISA_L_BARO_SENS 1.0 #endif #ifndef LISA_L_DIFF_SENS #define LISA_L_DIFF_SENS 1.0 #endif #ifndef LISA_L_BARO_SENDER_ID #define LISA_L_BARO_SENDER_ID 2 #endif #ifdef BARO_LED LED_OFF BARO_LED #endif baro_board.running = FALSE baro_board.running = TRUE #ifdef BARO_LED if baro_board.running == TRUE LED_ON BARO_LED else LED_TOGGLE BARO_LED #endif void lisa_l_baro_event void if baro_board.status == LBS_READING_ABS && baro_trans.status != I2CTransPending baro_board.status = LBS_READ_ABS if baro_trans.status == I2CTransSuccess int16_t tmp = baro_trans.buf 0 <<8 | baro_trans.buf 1 float pressure = LISA_L_BARO_SENS* float tmp AbiSendMsgBARO_ABS LISA_L_BARO_SENDER_ID &pressure else if baro_board.status == LBS_READING_DIFF && baro_trans.status != I2CTransPending baro_board.status = LBS_READ_DIFF if baro_trans.status == I2CTransSuccess int16_t tmp = baro_trans.buf 0 <<8 | baro_trans.buf 1 float diff = LISA_L_DIFF_SENS* float tmp AbiSendMsgBARO_DIFF LISA_L_BARO_SENDER_ID &diff static inline void baro_board_send_config_abs void static inline void baro_board_send_config_diff void static inline void baro_board_send_reset void #include std.h #include subsystems/abi.h #ifndef LISA_M_BARO_SENDER_ID #define LISA_M_BARO_SENDER_ID 6 #endif #ifdef BARO_LED LED_OFF BARO_LED #endif else float pressure = float baro_bmp085.pressure AbiSendMsgBARO_ABS UMARIM_BARO_SENDER_ID &pressure #ifdef BARO_LED RunOnceEvery 10 LED_TOGGLE BARO_LED "
Paparazzi 195 baro_board.c Non-Control 5c7f9790b7 [config] consistent foo_I2C_DEV and PRINT_CONFIG " INFO Using High LisaL Baro Gain: Do not use below 1000hPa INFO Using Low LisaL Baro Gain capable of measuring below 1000hPa or more "
Paparazzi 196 baro_board.c Non-Control fff541d6a3 Merge branch dev into dev_i2c* this is for testing of the new i2c driver as long as locm3 is not far enough* has all the shebang from current dev so positive gains rc roll etc.. ""
Paparazzi 197 baro_board.c Non-Control 8d0251451e Merge branch master of github.com:paparazzi/paparazzi into 4.0_beta ""
Paparazzi 198 baro_board.c Non-Control aa88480860 Cleanspaces " #pragma message Using Low LisaL Baro Gain capable of measuring below 1000hPa or more baro_board_write_to_register BARO_ABS_ADDR 0x01 0x84 0x83 "
Paparazzi 199 baro_board.c Control eeb266d021 lisa_l baro: enable it again " if !i2c_idle &i2c2 return "
Paparazzi 200 baro_board.c Non-Control 17bbb1eda7 Merge branch stm_i2c into dev_i2c ""
Paparazzi 201 baro_board.c Non-Control 37cf82d386 Merge branch master of github.com:paparazzi/paparazzi into stm_i2c ""
Paparazzi 202 baro_board.c Non-Control 9f8163ef42 Merge branch master into dev ""
Paparazzi 203 baro_board.c Control 697593f621 Baro on Lisa-L needs to be set in low gain when pressure is low " #ifndef BARO_LOW_GAIN #pragma message Using High LisaL Baro Gain: Do not use below 1000hPa #else #pragma message Using Low LisaL Baro Gain capable of measuring below 1000hPa or more baro_board_write_to_register BARO_ABS_ADDR 0x01 0x84 0x83 #endif"
Paparazzi 204 baro_board.c Non-Control ae606a13b2 Merge branch master of github.com:paparazzi/paparazzi into stm_i2c ""
Paparazzi 205 baro_board.c Control 59907827a4 removed baro_downlink_raw from lisa_l baro_board use normal periodic send ""
Paparazzi 206 baro_board.c Control 33d59cd8dd some more downlink macros adapted " DOWNLINK_SEND_BARO_RAW DefaultChannel DefaultDevice &baro.absolute &baro.differential "
Paparazzi 207 baro_board.c Non-Control bdce5f123b fix inlude of downlink for lisa/l baro board " #include subsystems/datalink/downlink.h"
Paparazzi 208 baro_board.c Non-Control d94719bef8 fix inlude of downlink for lisa/l baro board " #include downlink.h"
Paparazzi 209 baro_board.c Control 98cb9c414a added baro_downlink_raw for lisa_l baro_board but this is not very nice... there should be no downlink in baro implementation " #include mcu_periph/uart.h #include messages.h #include subsystems/datalink/downlink.h #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif void baro_downlink_raw void DOWNLINK_SEND_BARO_RAW DefaultChannel &baro.absolute &baro.differential "
Paparazzi 210 baro_board.c Non-Control 9f52491f31 :-) :-) :-) :-) " #warning BARO_BOARD_CODE_DISABLED"
Paparazzi 211 baro_board.c Control 0118e49e20 Disable other interrupt while first is running " #warning BARO_FILE_CODE_DISABLED"
Paparazzi 212 baro_board.c Non-Control e06da71f7d Whitespaces " return "
Paparazzi 213 baro_board.c Control 12629f5cbf Pulled stm_i2c_pullable into dev-branch stm_i2c ""
Paparazzi 214 baro_board.c Control 6988843baf @Allen: I have an idea why you added this but it should not be needed anymore ""
Paparazzi 215 baro_board.c Control 817407cc5b start with 1 driver at a time " return "
Paparazzi 216 baro_board.c Control fa88414f1d Don t submit more baro i2c requests if the bus is busy " if !i2c_idle &i2c2 return "
Paparazzi 217 baro_board.c Non-Control e90467ba86 moved firmwares/rotorcraft/baro to subsystems/sensors/baro " #include subsystems/sensors/baro.h #include subsystems/sensors/baro.h #include subsystems/sensors/baro.h"
Paparazzi 218 baro_board.c Non-Control 19199755fb cleanup a LOT of trailing whitespaces " * * Boston MA 02111-1307 USA. #define BARO_ABS_ADDR 0x90"
Paparazzi 219 baro_board.c Non-Control 08716bbd8d use double quotes for subsystems and firmware angle brackets only for external includes " #include firmwares/rotorcraft/baro.h #include firmwares/rotorcraft/baro.h #include firmwares/rotorcraft/baro.h"
Paparazzi 220 baro_board.c Non-Control 43839b5b7e use angle brackets for firmware includes " #include #include #include "
Paparazzi 221 baro_board.c Control 9cb3001ba2 breaking I2C - don t update now work in progress " struct i2c_transaction baro_trans void baro_board_send_reset void baro_trans.type = I2CTransTx baro_trans.slave_addr = 0x00 baro_trans.len_w = 1 baro_trans.buf 0 = 0x06 i2c_submit &i2c2 &baro_trans baro_trans.type = I2CTransTx baro_trans.slave_addr = baro_addr baro_trans.len_w = 3 baro_trans.buf 0 = reg_addr baro_trans.buf 1 = val_msb baro_trans.buf 2 = val_lsb i2c_submit &i2c2 &baro_trans baro_trans.type = I2CTransTxRx baro_trans.slave_addr = baro_addr baro_trans.len_w = 1 baro_trans.len_r = 2 baro_trans.buf 0 = reg_addr i2c_submit &i2c2 &baro_trans baro_trans.type = I2CTransTx baro_trans.slave_addr = baro_addr baro_trans.len_w = 1 baro_trans.buf 0 = reg_addr i2c_submit &i2c2 &baro_trans baro_trans.type = I2CTransRx baro_trans.slave_addr = baro_addr baro_trans.len_r = 2 i2c_submit &i2c2 &baro_trans "
Paparazzi 222 baro_board.c Non-Control 0c80fd0db7 moved booz/booz2_main.c to firmwares/rotorcraft/main.c " #include firmwares/rotorcraft/baro.h #include firmwares/rotorcraft/baro.h"
Paparazzi 223 baro_board.c Control 923a2e92ce had forgoten to pass the status to RUNNING after initialization " baro.status = BS_RUNNING "
Paparazzi 225 guidance_h.c Non-Control 9e5a645120 [build] fix some of the implicit-fallthrough warnings from GCC7Some warnings related to the use of the Label() macro are still there.It seems that the only way would be generate the labels instead ofcalling the macros.See issue #2207 ""
Paparazzi 226 guidance_h.c Control 86baffe98a guidance_h heading setpoint in float (#2051) " FLOAT_EULERS_ZERO guidance_h.rc_sp guidance_h.sp.heading = 0.0 guidance_h.sp.heading_rate = 0.0 stabilization_attitude_read_rc_setpoint_eulers_f &guidance_h.rc_sp in_flight FALSE FALSE stabilization_attitude_read_rc_setpoint_eulers_f &guidance_h.rc_sp in_flight FALSE FALSE FLOAT_EULERS_ZERO guidance_h.rc_sp guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY FLOAT_ANGLE_NORMALIZE guidance_h.sp.heading guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f ->psi guidance_h.sp.heading = stateGetNedToBodyEulers_f ->psi guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP nav_heading FLOAT_ANGLE_NORMALIZE guidance_h.sp.heading int32_t heading_sp_i = ANGLE_BFP_OF_REAL guidance_h.sp.heading heading_sp_i int32_t heading_sp_i = ANGLE_BFP_OF_REAL guidance_h.sp.heading stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth heading_sp_i guidance_h.sp.heading = heading FLOAT_ANGLE_NORMALIZE guidance_h.sp.heading guidance_h.sp.heading_rate = rate "
Paparazzi 227 guidance_h.c Control 9792de5704 [hybrid vehicles] gradually transition back from forward flight (#2003)Before if you wanted to go back from forward flight to hover the pitch angle setpoint would jump back to hover. Some vehicles can handle this but possibly not all. With this commit when you switch back the setpoint will gradually come up again just like in the transition to forward flight. Also add the attitude visualizer message to the indi controller " static inline void transition_run bool to_forward transition_run true if ! guidance_h.mode == GUIDANCE_H_MODE_FORWARD && transition_percentage > 0 transition_run false static inline void transition_run bool to_forward if to_forward transition_percentage += 1 << INT32_PERCENTAGE_FRAC - 4 else transition_percentage -= 1 << INT32_PERCENTAGE_FRAC - 4 "
Paparazzi 228 guidance_h.c Non-Control 3048259ea8 [modules] convert GNC susystems to modules for rotorcraft ""
Paparazzi 229 guidance_h.c Non-Control 5613d79dc4 [autopilot] support generated autopilot based on rotorcraft firmwareBy default the original static autopilot is used. A config flag canenable the use of the generated AP code.A basic autopilot description is provided (4 modes + failsafe modes).The server is capable of using the list of generated mode to properlydisplay mode names.Tested in NAV and GUIDED mode in sim and direct attitude control onreal aircraft. " guidance_h_guided_run in_flight guidance_h_from_nav in_flight void guidance_h_hover_enter void void guidance_h_nav_enter void void guidance_h_from_nav bool in_flight if !in_flight guidance_h_nav_enter if horizontal_mode == HORIZONTAL_MODE_MANUAL stabilization_cmd COMMAND_ROLL = nav_cmd_roll stabilization_cmd COMMAND_PITCH = nav_cmd_pitch stabilization_cmd COMMAND_YAW = nav_cmd_yaw else if horizontal_mode == HORIZONTAL_MODE_ATTITUDE struct Int32Eulers sp_cmd_i sp_cmd_i.phi = nav_roll sp_cmd_i.theta = nav_pitch sp_cmd_i.psi = nav_heading stabilization_attitude_set_rpy_setpoint_i &sp_cmd_i stabilization_attitude_run in_flight #if HYBRID_NAVIGATION guidance_hybrid_reset_heading &sp_cmd_i #endif else #if HYBRID_NAVIGATION INT32_VECT2_NED_OF_ENU guidance_h.sp.pos navigation_target guidance_hybrid_run #else INT32_VECT2_NED_OF_ENU guidance_h.sp.pos navigation_carrot guidance_h_update_reference guidance_h.sp.heading = nav_heading INT32_ANGLE_NORMALIZE guidance_h.sp.heading #if GUIDANCE_INDI guidance_indi_run in_flight guidance_h.sp.heading #else guidance_h_traj_run in_flight stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth guidance_h.sp.heading #endif #endif stabilization_attitude_run in_flight void guidance_h_guided_run bool in_flight if !in_flight guidance_h_hover_enter guidance_h_update_reference #if GUIDANCE_INDI guidance_indi_run in_flight guidance_h.sp.heading #else guidance_h_traj_run in_flight stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth guidance_h.sp.heading #endif stabilization_attitude_run in_flight "
Paparazzi 230 guidance_h.c Control 1a53c4471a set nav cmds instead of angles in nav manual mode (#1839)* set nav cmds instead of angles in nav manual mode * updated doxygen documentation for the nav_set_manual function " stabilization_cmd COMMAND_ROLL = nav_cmd_roll stabilization_cmd COMMAND_PITCH = nav_cmd_pitch stabilization_cmd COMMAND_YAW = nav_cmd_yaw "
Paparazzi 231 guidance_h.c Non-Control a88161d10c fix hover enter to position hold not zero velocity hold " guidance_h.sp.speed.x = 0 guidance_h.sp.speed.y = 0 "
Paparazzi 232 guidance_h.c Non-Control ea7b0b9432 [rotorcraft] fix set_guided_body_vel (#1775) " float newvx = cosf -psi * vx + sinf -psi * vy float newvy = -sinf -psi * vx + cosf -psi * vy return guidance_h_set_guided_vel newvx newvy "
Paparazzi 233 guidance_h.c Control f1f75e96b8 Hybrid guidance code for hybrid drones (#1769)This pull request is for the guidance algorithms used to control hybrid drones like the Quadshot and MavShot. It supports autonomous waypoint navigation as well as forward-flight . The code was flight tested in autonomous waypoint navigation mode including autonomous landing (using sonar). " #include firmwares/rotorcraft/guidance/guidance_hybrid.h #if HYBRID_NAVIGATION guidance_hybrid_init #endif #if HYBRID_NAVIGATION guidance_hybrid_norm_ref_airspeed = 0 #endif #if HYBRID_NAVIGATION guidance_hybrid_reset_heading &sp_cmd_i #endif #if HYBRID_NAVIGATION INT32_VECT2_NED_OF_ENU guidance_h.sp.pos navigation_target guidance_hybrid_run #else #endif const struct Int32Vect2 *guidance_h_get_pos_err void return &guidance_h_pos_err "
Paparazzi 234 guidance_h.c Control 0135507d28 [guidance] added helper function to set velocity in body frame when in guided mode " bool guidance_h_set_guided_body_vel float vx float vy float psi = stateGetNedToBodyEulers_f ->psi vx = cosf -psi * vx + sinf -psi * vy vy = -sinf -psi * vx + cosf -psi * vy return guidance_h_set_guided_vel vx vy "
Paparazzi 235 guidance_h.c Control 6686c9a0ce [guidance] Expansion of the guided protocol (#1694)Updates the GUIDED_SETPOINT_NED message to set the reference frame of each input separately and adds a heading_rate setpoint. Frame can be specified with the bits 0-3 Velocity of position setpoint can be specified with the bits 5-7 Flags field definition: - bit 0: x y as offset coordinates - bit 1: x y in body coordinates - bit 2: z as offset coordinates - bit 3: yaw as offset coordinates - bit 4: free - bit 5: x y as vel - bit 6: z as vel - bit 7: yaw as rate " guidance_h.sp.heading_rate = 0 case GUIDANCE_H_MODE_GUIDED: if horizontal_mode == HORIZONTAL_MODE_MANUAL else if horizontal_mode == HORIZONTAL_MODE_ATTITUDE if bit_is_set guidance_h.sp.mask 5 if bit_is_set guidance_h.sp.mask 7 guidance_h.sp.heading += guidance_h.sp.heading_rate >> INT32_ANGLE_FRAC - INT32_RATE_FRAC / PERIODIC_FREQUENCY INT32_ANGLE_NORMALIZE guidance_h.sp.heading guidance_h.gains.v * guidance_h.ref.speed.x >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h.gains.a * guidance_h.ref.accel.x >> INT32_ACCEL_FRAC - GH_GAIN_SCALE guidance_h.gains.v * guidance_h.ref.speed.y >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h.gains.a * guidance_h.ref.accel.y >> INT32_ACCEL_FRAC - GH_GAIN_SCALE VECT2_STRIM guidance_h_trim_att_integrator - traj_max_bank << INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2 traj_max_bank << INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2 guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2 guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2 ClearBit guidance_h.sp.mask 7 guidance_h_set_guided_vel 0. 0. ClearBit guidance_h.sp.mask 7 bool guidance_h_set_guided_heading_rate float rate if guidance_h.mode == GUIDANCE_H_MODE_GUIDED SetBit guidance_h.sp.mask 7 guidance_h.sp.heading_rate = RATE_BFP_OF_REAL rate return true return false "
Paparazzi 236 guidance_h.c Control a9e9be2dab [heli][flight_plans] Heli Spinup routine and flight plan mode manual (#1606) " if horizontal_mode == HORIZONTAL_MODE_MANUAL stabilization_cmd COMMAND_ROLL = nav_roll stabilization_cmd COMMAND_PITCH = nav_pitch stabilization_cmd COMMAND_YAW = nav_heading else if horizontal_mode == HORIZONTAL_MODE_ATTITUDE stabilization_attitude_run in_flight stabilization_attitude_run in_flight "
Paparazzi 237 guidance_h.c Non-Control 0f53c6f270 [rotorcraft] guidance_h: use shift defines instead of hardcoded numbersand fixed some typoscloses part of #1634 " guidance_h.gains.v * guidance_h.ref.speed.x >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h.gains.a * guidance_h.ref.accel.x >> INT32_ACCEL_FRAC - GH_GAIN_SCALE guidance_h.gains.v * guidance_h.ref.speed.y >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h.gains.a * guidance_h.ref.accel.y >> INT32_ACCEL_FRAC - GH_GAIN_SCALE VECT2_STRIM guidance_h_trim_att_integrator - traj_max_bank << INT32_ANGLE_FRAC + GH_GAIN_SCALE*2 traj_max_bank << INT32_ANGLE_FRAC + GH_GAIN_SCALE*2 guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> INT32_ANGLE_FRAC + GH_GAIN_SCALE*2 guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> INT32_ANGLE_FRAC + GH_GAIN_SCALE*2 "
Paparazzi 238 guidance_h.c Non-Control 8edab84dd4 replace TRUE/FALSE by stdbool true/false " return true return false return true return false return true return false "
Paparazzi 239 guidance_h.c Non-Control 5c1e4260fe replace bool_t with C99 bool from stdbool.h " static void guidance_h_traj_run bool in_flight static void read_rc_setpoint_speed_i struct Int32Vect2 *speed_sp bool in_flight void guidance_h_read_rc bool in_flight void guidance_h_run bool in_flight static void guidance_h_traj_run bool in_flight static void read_rc_setpoint_speed_i struct Int32Vect2 *speed_sp bool in_flight bool guidance_h_set_guided_pos float x float y bool guidance_h_set_guided_heading float heading bool guidance_h_set_guided_vel float vx float vy "
Paparazzi 240 guidance_h.c Non-Control 1e29cb623f [rotorcraft] fix velocity command in HOVER modeThis was a regression accidentally introduced in af1739b54c3a02b67ac5e919b7e38a95e12e1adcFixes #1549 " SetBit guidance_h.sp.mask 4 SetBit guidance_h.sp.mask 5 else "
Paparazzi 241 guidance_h.c Control a3a641c1ee [stabilization] INDI rewrite and rate control " #if USE_STABILIZATION_RATE #endif #if USE_STABILIZATION_RATE #endif #if USE_STABILIZATION_RATE #endif"
Paparazzi 242 guidance_h.c Control af1739b54c [rotorcraft] start adding velocity commands for guided mode " if bit_is_set guidance_h.sp.mask 4 && bit_is_set guidance_h.sp.mask 5 ClearBit guidance_h.sp.mask 4 ClearBit guidance_h.sp.mask 5 ClearBit guidance_h.sp.mask 4 ClearBit guidance_h.sp.mask 5 ClearBit guidance_h.sp.mask 4 ClearBit guidance_h.sp.mask 5 ClearBit guidance_h.sp.mask 7 bool_t guidance_h_set_guided_vel float vx float vy if guidance_h.mode == GUIDANCE_H_MODE_GUIDED SetBit guidance_h.sp.mask 4 SetBit guidance_h.sp.mask 5 guidance_h.sp.speed.x = SPEED_BFP_OF_REAL vx guidance_h.sp.speed.y = SPEED_BFP_OF_REAL vy return TRUE return FALSE "
Paparazzi 243 guidance_h.c Control 85777e86cd [rotorcraft] add a GUIDED modemeant for controlling the rotorcraft via external input (from a module or datalink message)Currently positions mode only:- specify frame via first 4 bits in flags: - 0x0: LOCAL_NED - 0x1: LOCAL_OFFSET_NED - 0x2: BODY_NED - 0x3: BODY_OFFSET_NED " guidance_h.sp.heading = guidance_h.rc_sp.psi case GUIDANCE_H_MODE_GUIDED: stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth guidance_h.sp.heading guidance_h.sp.heading = guidance_h.rc_sp.psi bool_t guidance_h_set_guided_pos float x float y if guidance_h.mode == GUIDANCE_H_MODE_GUIDED guidance_h.sp.pos.x = POS_BFP_OF_REAL x guidance_h.sp.pos.y = POS_BFP_OF_REAL y return TRUE return FALSE bool_t guidance_h_set_guided_heading float heading if guidance_h.mode == GUIDANCE_H_MODE_GUIDED guidance_h.sp.heading = ANGLE_BFP_OF_REAL heading INT32_ANGLE_NORMALIZE guidance_h.sp.heading return TRUE return FALSE "
Paparazzi 244 guidance_h.c Non-Control faf2206c32 [telemetry] register_periodic: use PPRZ_MSG_ID_x instead of DL_x " register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_GUIDANCE_H_INT send_gh register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_HOVER_LOOP send_hover_loop register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_GUIDANCE_H_REF_INT send_href register_periodic_telemetry DefaultPeriodic PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER send_tune_hover "
Paparazzi 245 guidance_h.c Non-Control 83065af402 Change register_periodic_telemetry() to use msg id " register_periodic_telemetry DefaultPeriodic DL_GUIDANCE_H_INT send_gh register_periodic_telemetry DefaultPeriodic DL_HOVER_LOOP send_hover_loop register_periodic_telemetry DefaultPeriodic DL_GUIDANCE_H_REF_INT send_href register_periodic_telemetry DefaultPeriodic DL_ROTORCRAFT_TUNE_HOVER send_tune_hover "
Paparazzi 246 guidance_h.c Non-Control 32fba7c45e [fix] fix most of the warnings found by CI servers " #if !GUIDANCE_INDI #endif #if !GUIDANCE_INDI #endif"
Paparazzi 247 guidance_h.c Non-Control f30db36838 Merge pull request #1354 from EwoudSmeur/outer_loop_to_master2outer loop INDI control ""
Paparazzi 248 guidance_h.c Control 8a938b394c added include and added heading support " #if GUIDANCE_INDI guidance_indi_run in_flight guidance_h.sp.heading #else guidance_indi_run in_flight guidance_h.sp.heading "
Paparazzi 249 guidance_h.c Control eac9c94a47 [guidance_h] In MODE_NAV submode ATTITUDE set the heading from nav_heading " sp_cmd_i.psi = nav_heading "
Paparazzi 250 guidance_h.c Control 60c27cccfd flying position hold " #include firmwares/rotorcraft/guidance/guidance_indi.h #ifndef GUIDANCE_INDI #define GUIDANCE_INDI FALSE #endif #if GUIDANCE_INDI guidance_indi_enter #endif #if GUIDANCE_INDI guidance_indi_enter #endif #if GUIDANCE_INDI guidance_indi_run in_flight #else #endif #if GUIDANCE_INDI guidance_indi_run in_flight #else #endif"
Paparazzi 251 guidance_h.c Control 1e4b0b6d91 [guidance] Add a flip guidance mode " #include firmwares/rotorcraft/guidance/guidance_flip.h case GUIDANCE_H_MODE_FLIP: guidance_flip_enter break case GUIDANCE_H_MODE_FLIP: stabilization_attitude_read_rc in_flight FALSE FALSE break case GUIDANCE_H_MODE_FLIP: guidance_flip_run break "
Paparazzi 252 guidance_h.c Control dab660ff55 add OPTICAL_FLOW and VELOCITY_ESTIMATE ABI messagesand use them in the opticflow modules " #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE guidance_h_module_init #endif"
Paparazzi 253 guidance_h.c Non-Control 0a0b119c8f [rotorcraft] guidance_h: put vars in struct " struct HorizontalGuidance guidance_h struct Int32Vect2 guidance_h_cmd_earth &guidance_h.sp.pos.x &guidance_h.sp.pos.y &guidance_h.ref.pos.x &guidance_h.ref.pos.y &guidance_h.sp.pos.x &guidance_h.sp.pos.y &guidance_h.sp.heading &guidance_h.sp.pos.x &guidance_h.ref.pos.x &guidance_h.sp.speed.x &guidance_h.ref.speed.x &guidance_h.ref.accel.x &guidance_h.sp.pos.y &guidance_h.ref.pos.y &guidance_h.sp.speed.y &guidance_h.ref.speed.y &guidance_h.ref.accel.y guidance_h.mode = GUIDANCE_H_MODE_KILL guidance_h.use_ref = GUIDANCE_H_USE_REF guidance_h.approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST INT_VECT2_ZERO guidance_h.sp.pos INT_EULERS_ZERO guidance_h.rc_sp guidance_h.sp.heading = 0 guidance_h.gains.p = GUIDANCE_H_PGAIN guidance_h.gains.i = GUIDANCE_H_IGAIN guidance_h.gains.d = GUIDANCE_H_DGAIN guidance_h.gains.a = GUIDANCE_H_AGAIN guidance_h.gains.v = GUIDANCE_H_VGAIN VECT2_COPY guidance_h.ref.pos *stateGetPositionNed_i VECT2_COPY guidance_h.ref.speed *stateGetSpeedNed_i INT_VECT2_ZERO guidance_h.ref.accel gh_set_ref guidance_h.ref.pos guidance_h.ref.speed guidance_h.ref.accel if new_mode == guidance_h.mode if guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT if guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT if guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT guidance_h.mode = new_mode switch guidance_h.mode stabilization_attitude_read_rc_setpoint_eulers &guidance_h.rc_sp in_flight FALSE FALSE read_rc_setpoint_speed_i &guidance_h.sp.speed in_flight stabilization_attitude_read_rc_setpoint_eulers &guidance_h.rc_sp in_flight FALSE FALSE INT_EULERS_ZERO guidance_h.rc_sp switch guidance_h.mode guidance_h.sp.heading = guidance_h.rc_sp.psi guidance_h.sp.heading INT32_VECT2_NED_OF_ENU guidance_h.sp.pos navigation_carrot guidance_h.sp.heading = nav_heading INT32_ANGLE_NORMALIZE guidance_h.sp.heading guidance_h.sp.heading if guidance_h.mode == GUIDANCE_H_MODE_HOVER gh_update_ref_from_speed_sp guidance_h.sp.speed gh_update_ref_from_pos_sp guidance_h.sp.pos if guidance_h.use_ref INT32_VECT2_RSHIFT guidance_h.ref.pos gh_ref.pos GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT guidance_h.ref.speed gh_ref.speed INT32_SPEED_FRAC - GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT guidance_h.ref.accel gh_ref.accel INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC VECT2_COPY guidance_h.ref.pos guidance_h.sp.pos INT_VECT2_ZERO guidance_h.ref.speed INT_VECT2_ZERO guidance_h.ref.accel if guidance_h.mode == GUIDANCE_H_MODE_HOVER VECT2_COPY guidance_h.sp.pos guidance_h.ref.pos VECT2_DIFF guidance_h_pos_err guidance_h.ref.pos *stateGetPositionNed_i VECT2_DIFF guidance_h_speed_err guidance_h.ref.speed *stateGetSpeedNed_i guidance_h.gains.p * guidance_h_pos_err.x >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h.gains.d * guidance_h_speed_err.x >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 guidance_h.gains.p * guidance_h_pos_err.y >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h.gains.d * guidance_h_speed_err.y >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 guidance_h_cmd_earth.x = pd_x + guidance_h.gains.v * guidance_h.ref.speed.x >> 17 + guidance_h.gains.a * guidance_h.ref.accel.x >> 8 guidance_h_cmd_earth.y = pd_y + guidance_h.gains.v * guidance_h.ref.speed.y >> 17 + guidance_h.gains.a * guidance_h.ref.accel.y >> 8 guidance_h_trim_att_integrator.x += guidance_h.gains.i * pd_x guidance_h_trim_att_integrator.y += guidance_h.gains.i * pd_y if guidance_h.approx_force_by_thrust && in_flight VECT2_COPY guidance_h.sp.pos *stateGetPositionNed_i guidance_h.rc_sp.psi = stateGetNedToBodyEulers_i ->psi INT32_VECT2_NED_OF_ENU guidance_h.sp.pos navigation_carrot guidance_h.gains.i = igain "
Paparazzi 254 guidance_h.c Non-Control a4275abee3 [rotorcraft] guidance_h: make some vars private " struct Int32Vect2 guidance_h_pos_err struct Int32Vect2 guidance_h_speed_err struct Int32Vect2 guidance_h_trim_att_integrator void guidance_h_set_igain uint32_t igain guidance_h_igain = igain INT_VECT2_ZERO guidance_h_trim_att_integrator "
Paparazzi 255 guidance_h.c Non-Control 0bc805da09 [rotorcraft] guidance: fix max bank angle limitthanks Eduardo for pointing this out " static const int32_t traj_max_bank = Min BFP_OF_REAL GUIDANCE_H_MAX_BANK INT32_ANGLE_FRAC "
Paparazzi 256 guidance_h.c Non-Control 7a0f034bd8 [module-ctrl] Generic framework to add many new types of controllers in a module " #include firmwares/rotorcraft/guidance/guidance_module.h #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_enter break #endif #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_read_rc break #endif #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_run in_flight #endif"
Paparazzi 257 guidance_h.c Control 98c27f257c [opticflow] controller " case GUIDANCE_H_MODE_MODULE_OUTERLOOP: case GUIDANCE_H_MODE_MODULE_OUTERLOOP: case GUIDANCE_H_MODE_MODULE_OUTERLOOP: if !in_flight guidance_h_nav_enter #if USE_MODULE_OUTERLOOP==1 guidance_module_run in_flight #endif break "
Paparazzi 258 guidance_h.c Non-Control 603b40a513 [airborne] fix code style on (nearly) all files```find sw/airborne/ -regextype posix-extended -regex sw/airborne/.*(chibios-libopencm3|lpcusb|efsl|lpc21/include|lpc21/test/bootloader|subsystems/ahrs) -prune -o -name *.[ch] -exec ./fix_code_style.sh {} +```ignored ahrs (for now) to not create unnecessary conflicts for some pending changes.. " static void send_gh struct transport_tx *trans struct link_device *dev struct NedCoor_i *pos = stateGetPositionNed_i &guidance_h_pos_sp.x &guidance_h_pos_sp.y &guidance_h_pos_ref.x &guidance_h_pos_ref.y & pos->x & pos->y static void send_hover_loop struct transport_tx *trans struct link_device *dev struct NedCoor_i *pos = stateGetPositionNed_i struct NedCoor_i *speed = stateGetSpeedNed_i struct NedCoor_i *accel = stateGetAccelNed_i static void send_href struct transport_tx *trans struct link_device *dev &guidance_h_pos_sp.x &guidance_h_pos_ref.x &guidance_h_speed_sp.x &guidance_h_speed_ref.x &guidance_h_accel_ref.x &guidance_h_pos_sp.y &guidance_h_pos_ref.y &guidance_h_speed_sp.y &guidance_h_speed_ref.y &guidance_h_accel_ref.y static void send_tune_hover struct transport_tx *trans struct link_device *dev &radio_control.values RADIO_ROLL &radio_control.values RADIO_PITCH &radio_control.values RADIO_YAW &stabilization_cmd COMMAND_ROLL &stabilization_cmd COMMAND_PITCH &stabilization_cmd COMMAND_YAW &stabilization_cmd COMMAND_THRUST & stateGetNedToBodyEulers_i ->phi & stateGetNedToBodyEulers_i ->theta & stateGetNedToBodyEulers_i ->psi void guidance_h_init void static inline void reset_guidance_reference_from_current_position void void guidance_h_mode_changed uint8_t new_mode if new_mode == guidance_h_mode transition_percentage = 0 transition_theta_offset = 0 void guidance_h_read_rc bool_t in_flight switch guidance_h_mode else void guidance_h_run bool_t in_flight switch guidance_h_mode if transition_percentage < 100 << INT32_PERCENTAGE_FRAC if !in_flight if !in_flight else static void guidance_h_update_reference void if guidance_h_mode == GUIDANCE_H_MODE_HOVER else gh_update_ref_from_pos_sp guidance_h_pos_sp INT32_VECT2_RSHIFT guidance_h_pos_ref gh_ref.pos GH_POS_REF_FRAC - INT32_POS_FRAC static void guidance_h_traj_run bool_t in_flight BFP_OF_REAL RadOfDeg 40 INT32_ANGLE_FRAC thrust_cmd_filt = thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust / GUIDANCE_H_THRUST_CMD_FILTER + 1 guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt static void guidance_h_hover_enter void static void guidance_h_nav_enter void static inline void transition_run void transition_percentage += 1 << INT32_PERCENTAGE_FRAC - 4 transition_theta_offset = INT_MULT_RSHIFT transition_percentage << INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC / 100 max_offset INT32_ANGLE_FRAC static void read_rc_setpoint_speed_i struct Int32Vect2 *speed_sp bool_t in_flight DeadBand rc_x MAX_PPRZ / 20 DeadBand rc_y MAX_PPRZ / 20 speed_sp->x = int32_t int64_t c_psi * rc_x + int64_t s_psi * rc_y >> INT32_TRIG_FRAC else "
Paparazzi 259 guidance_h.c Non-Control 34b6ca3a26 [telemetry] rename device to link_device " static void send_gh struct transport_tx *trans struct link_device *dev static void send_hover_loop struct transport_tx *trans struct link_device *dev static void send_href struct transport_tx *trans struct link_device *dev static void send_tune_hover struct transport_tx *trans struct link_device *dev "
Paparazzi 260 guidance_h.c Non-Control e5cdfa1ce1 [telemetry] convert macros in registered callback to functions " pprz_msg_send_GUIDANCE_H_INT trans dev AC_ID pprz_msg_send_HOVER_LOOP trans dev AC_ID pprz_msg_send_GUIDANCE_H_REF_INT trans dev AC_ID pprz_msg_send_ROTORCRAFT_TUNE_HOVER trans dev AC_ID "
Paparazzi 261 guidance_h.c Non-Control c4b73552cd [telemetry] change register callback to pass arguments " static void send_gh struct transport_tx *trans struct device *dev static void send_hover_loop struct transport_tx *trans struct device *dev static void send_href struct transport_tx *trans struct device *dev static void send_tune_hover struct transport_tx *trans struct device *dev "
Paparazzi 262 guidance_h.c Non-Control 0c64a410c0 [rotorcraft] reorg stabilization includes/inittowards not requiring any specfic stabilization controllers (none rate attitude) " #include firmwares/rotorcraft/stabilization/stabilization_none.h #include firmwares/rotorcraft/stabilization/stabilization_rate.h #include firmwares/rotorcraft/stabilization/stabilization_attitude.h"
Paparazzi 263 guidance_h.c Non-Control 70b08f099c [rotorcraft] horizontal guidance reference refactormake the reference model adjustable at runtime via settings " gh_ref_init INT32_VECT2_RSHIFT guidance_h_pos_ref gh_ref.pos GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT guidance_h_speed_ref gh_ref.speed INT32_SPEED_FRAC - GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT guidance_h_accel_ref gh_ref.accel INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC "
Paparazzi 264 guidance_h.c Non-Control 843ea9b850 [rotorcraft] add speed sp to GUIDANCE_H_REF_INT messagecloses #763 " &guidance_h_speed_sp.x &guidance_h_speed_ref.x &guidance_h_accel_ref.x &guidance_h_speed_sp.y &guidance_h_speed_ref.y &guidance_h_accel_ref.y "
Paparazzi 265 guidance_h.c Non-Control 30b7bd7b82 [rotorcraft] guidance_h: fix typo in y vgain " guidance_h_vgain * guidance_h_speed_ref.y >> 17 + "
Paparazzi 266 guidance_h.c Non-Control 96778c01f3 [rotorcraft] include and dox cleanup " #include generated/airframe.h #include firmwares/rotorcraft/guidance/guidance_h.h #include subsystems/radio_control.h"
Paparazzi 267 guidance_h.c Non-Control 9bcccc6f51 [rotorcraft] guidance_h speed ref saturation fixBound ref to max accel along reference vector.If angle can t be computed simply set both axes to max magnitude instead of zero.Should (tm) fix #716 " if guidance_h_mode == GUIDANCE_H_MODE_HOVER if in_flight "
Paparazzi 268 guidance_h.c Control 3830f136e0 [guidance] fix rotation matrix " speed_sp->x = int32_t int64_t c_psi * rc_x + int64_t s_psi * rc_y >> INT32_TRIG_FRAC speed_sp->y = int32_t - int64_t s_psi * rc_x + int64_t c_psi * rc_y >> INT32_TRIG_FRAC "
Paparazzi 269 guidance_h.c Control 5de51d3558 [guidance] fix IGAIN precision and add VGAINbased on #682this may introduce too large horizontal guidance IGAIN in rotorcraftairframe files " #ifndef GUIDANCE_H_AGAIN #define GUIDANCE_H_AGAIN 0 #endif #ifndef GUIDANCE_H_VGAIN #define GUIDANCE_H_VGAIN 0 #endif GUIDANCE_H_IGAIN < 0 || \ GUIDANCE_H_AGAIN < 0 || \ GUIDANCE_H_VGAIN < 0 int32_t guidance_h_vgain guidance_h_vgain = GUIDANCE_H_VGAIN guidance_h_vgain * guidance_h_speed_ref.x >> 17 + guidance_h_again * guidance_h_accel_ref.x >> 8 guidance_h_vgain * guidance_h_speed_ref.x >> 17 + guidance_h_again * guidance_h_accel_ref.y >> 8 guidance_h_trim_att_integrator.x += guidance_h_igain * pd_x guidance_h_trim_att_integrator.y += guidance_h_igain * pd_y VECT2_STRIM guidance_h_trim_att_integrator - traj_max_bank << 16 traj_max_bank << 16 guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> 16 guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> 16 "
Paparazzi 270 guidance_h.c Control f4b8517ed5 [rotorcraft] rc setpoint handling refactoredcloses #684 which partially solves #599 " stabilization_attitude_read_rc in_flight TRUE FALSE break case GUIDANCE_H_MODE_FORWARD: stabilization_attitude_read_rc in_flight FALSE TRUE break stabilization_attitude_read_rc in_flight FALSE FALSE stabilization_attitude_read_rc_setpoint_eulers &guidance_h_rc_sp in_flight FALSE FALSE stabilization_attitude_read_rc_setpoint_eulers &guidance_h_rc_sp in_flight FALSE FALSE "
Paparazzi 271 guidance_h.c Non-Control eba60ba1fe [telemetry] make specific flag for periodic telemetry " #if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY"
Paparazzi 272 guidance_h.c Control e3830578fb [telemetry] fix some remaining telemetry messages " static void send_tune_hover void DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER DefaultChannel DefaultDevice &radio_control.values RADIO_ROLL &radio_control.values RADIO_PITCH &radio_control.values RADIO_YAW &stabilization_cmd COMMAND_ROLL &stabilization_cmd COMMAND_PITCH &stabilization_cmd COMMAND_YAW &stabilization_cmd COMMAND_THRUST & stateGetNedToBodyEulers_i ->phi & stateGetNedToBodyEulers_i ->theta & stateGetNedToBodyEulers_i ->psi register_periodic_telemetry DefaultPeriodic ROTORCRAFT_TUNE_HOVER send_tune_hover "
Paparazzi 273 guidance_h.c Non-Control c384bc158f Merge branch master into telemetryfix ahrs_gx3 and stabilization_attitude_euler_float ""
Paparazzi 274 guidance_h.c Non-Control 3c7c08d16f [rotorcraft][guidance_h] GUIDANCE_H_USE_SPEED_REF defaults to TRUEThis also allows to give velocity commands via RC in GUIDANCE_H_MODE_HOVER. " PRINT_CONFIG_VAR GUIDANCE_H_USE_SPEED_REF "
Paparazzi 275 guidance_h.c Non-Control 72ea3b6aa6 [telemetry] update HOVER_LOOPremove unused cmd_body and nav_err " &guidance_h_pos_sp.x &guidance_h_pos_sp.y & pos->x & pos->y & speed->x & speed->y & accel->x & accel->y &guidance_h_pos_err.x &guidance_h_pos_err.y &guidance_h_speed_err.x &guidance_h_speed_err.y &guidance_h_trim_att_integrator.x &guidance_h_trim_att_integrator.y &guidance_h_cmd_earth.x &guidance_h_cmd_earth.y &guidance_h_heading_sp "
Paparazzi 276 guidance_h.c Non-Control 8ac5fd8d5b Merge branch master into telemetry* master: (125 commits) [abi] define ABI x_SENDER_IDs in abi_sender_ids.h [sim] flight gear viz: daytime on all spots on the planet not only France [abi] split abi messages from telemetry messages [fix] fix typo in actuators asctec v2 [baro] fix baro scaling for booz and navgo [boards] sim pc: only USE_BARO_BOARD for nps [conf] move some airframes to obsolete [modules] add baro_sim [sim] remove unused sim_baro [apogee] fix baro mpl3115 init on apogee Update README for recommended gcc-arm-embedded toolchain [boards] stm32f4_discovery typo in description [boards] roughly correct BOOZ_BARO_SENS [modules] baro_ets: BARO_ETS_SCALE [modules] baro modules for real sensors only for ap target not sim [conf] remove baro_board.xml module file [boards] update stm32f4_discovery [lib/ocaml] update leap_seconds [mission] fix segmentation fault and add generic mission FP [generator] generate arrays of the settings and fp blocks names ...Conflicts:sw/airborne/firmwares/fixedwing/ap_downlink.hsw/airborne/firmwares/fixedwing/main_ap.csw/airborne/firmwares/rotorcraft/telemetry.hsw/airborne/modules/sensors/baro_MS5534A.c ""
Paparazzi 277 guidance_h.c Control 9077861210 [rotorcraft] guidance_h typo in saturation and better resolution of the integrator " guidance_h_pgain * guidance_h_pos_err.x >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.x >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 guidance_h_pgain * guidance_h_pos_err.y >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.y >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 guidance_h_cmd_earth.x = pd_x + guidance_h_cmd_earth.y = pd_y + guidance_h_trim_att_integrator.x += guidance_h_igain * pd_x >> 8 guidance_h_trim_att_integrator.y += guidance_h_igain * pd_y >> 8 VECT2_STRIM guidance_h_trim_att_integrator - traj_max_bank << 12 traj_max_bank << 12 guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> 12 guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> 12 VECT2_STRIM guidance_h_cmd_earth -total_max_bank total_max_bank "
Paparazzi 278 guidance_h.c Control b6c2a8fef4 [rotorcraft] guidance_h Resolution of integrator increased AGain not in integration. " int32_t pd_x = guidance_h_pgain * guidance_h_pos_err.x >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.x >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 int32_t pd_y = guidance_h_pgain * guidance_h_pos_err.y >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.y >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 guidance_h_cmd_earth.x = pd_x + guidance_h_again * guidance_h_accel_ref.x >> 8 guidance_h_cmd_earth.y = pd_y + guidance_h_again * guidance_h_accel_ref.y >> 8 guidance_h_trim_att_integrator.x += guidance_h_igain * pd_x >> 6 >> 12 guidance_h_trim_att_integrator.y += guidance_h_igain * pd_y >> 6 >> 12 "
Paparazzi 279 guidance_h.c Control de12d2f4bb [rotorcraft] guidance_h total_max_bank " static const int32_t total_max_bank = BFP_OF_REAL RadOfDeg 45 INT32_ANGLE_FRAC VECT2_STRIM guidance_h_cmd_earth -traj_max_bank traj_max_bank guidance_h_trim_att_integrator.x += guidance_h_igain * guidance_h_cmd_earth.x >> 10 guidance_h_trim_att_integrator.y += guidance_h_igain * guidance_h_cmd_earth.y >> 10 guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y if guidance_h_approx_force_by_thrust && in_flight static int32_t thrust_cmd_filt int32_t vertical_thrust = stabilization_cmd COMMAND_THRUST * guidance_v_thrust_coeff >> INT32_TRIG_FRAC thrust_cmd_filt = thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust / GUIDANCE_H_THRUST_CMD_FILTER + 1 guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt VECT2_STRIM guidance_h_cmd_earth total_max_bank total_max_bank "
Paparazzi 280 guidance_h.c Control 8a3b8b4991 [rotorcraft] guidance_h: Updated integrator dynamicsIntegrate twice as fast when not only POS but also SPEED are wrong but do not integrate POS errors when the SPEED is already catching up. " if in_flight guidance_h_trim_att_integrator.x += guidance_h_igain * guidance_h_cmd_earth.x >> 10 guidance_h_trim_att_integrator.y += guidance_h_igain * guidance_h_cmd_earth.y >> 10 VECT2_STRIM guidance_h_trim_att_integrator -traj_max_bank traj_max_bank else INT_VECT2_ZERO guidance_h_trim_att_integrator "
Paparazzi 281 guidance_h.c Non-Control cad1ca2ad2 [rotorcraft] guidance_h: Rename sum err for clarity " struct Int32Vect2 guidance_h_trim_att_integrator INT_VECT2_ZERO guidance_h_trim_att_integrator INT_VECT2_ZERO guidance_h_trim_att_integrator guidance_h_trim_att_integrator.x += guidance_h_igain * guidance_h_pos_err.x >> 4 guidance_h_trim_att_integrator.y += guidance_h_igain * guidance_h_pos_err.y >> 4 VECT2_STRIM guidance_h_trim_att_integrator -traj_max_bank traj_max_bank INT_VECT2_ZERO guidance_h_trim_att_integrator guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y "
Paparazzi 282 guidance_h.c Control 45ff6dbc11 [rotorcraft] guidance_h MAX_BANKIn case of wind the -20 to 20 max bank should become -40 deg in the wind direction and +0 inthe opposite direction. The i-gain should be sufficiently SLOW but with HIGH authority.This commit suggests to split the max-bank from PD an I with separate saturation and have theI-gain before the integrator so better insight is gained in its authority. " guidance_h_pos_err_sum.x += guidance_h_igain * guidance_h_pos_err.x >> 4 guidance_h_pos_err_sum.y += guidance_h_igain * guidance_h_pos_err.y >> 4 VECT2_STRIM guidance_h_pos_err_sum -traj_max_bank traj_max_bank guidance_h_cmd_earth.x += guidance_h_pos_err_sum.x guidance_h_cmd_earth.y += guidance_h_pos_err_sum.y "
Paparazzi 283 guidance_h.c Non-Control 3e92315488 Merge remote-tracking branch paparazzi/master into telemetryConflicts:conf/firmwares/subsystems/fixedwing/autopilot.makefilesw/airborne/boards/ardrone/navdata.csw/airborne/boards/lisa_m/baro_ms5611_i2c.csw/airborne/boards/lisa_m/baro_ms5611_spi.csw/airborne/firmwares/fixedwing/ap_downlink.hsw/airborne/firmwares/fixedwing/main_ap.csw/airborne/firmwares/rotorcraft/telemetry.hsw/airborne/mcu_periph/i2c.csw/airborne/subsystems/ahrs/ahrs_float_cmpl.csw/airborne/subsystems/ahrs/ahrs_float_lkf.csw/airborne/subsystems/datalink/downlink.hsw/tools/gen_periodic.ml ""
Paparazzi 284 guidance_h.c Control fc2a471d6f [rotorcraft] fix hover speed RC setpoint- fix rc_x and rc_y values - still needs to be scaled to max speed using circle instead of box bounding but while making sure a division by zero can t occur!- only copy pos_ref to pos_sp in hover mode- move GUIDANCE_H_REF_MAX_SPEED|ACCEL to ref header file so it can be used- refactor rc speed setpoint reading into separate functioncloses #543 " #if GUIDANCE_H_USE_SPEED_REF static void read_rc_setpoint_speed_i struct Int32Vect2 *speed_sp bool_t in_flight #if GUIDANCE_H_USE_SPEED_REF read_rc_setpoint_speed_i &guidance_h_speed_sp in_flight #if GUIDANCE_H_USE_SPEED_REF if guidance_h_mode == GUIDANCE_H_MODE_HOVER VECT2_COPY guidance_h_pos_sp guidance_h_pos_ref #endif static void read_rc_setpoint_speed_i struct Int32Vect2 *speed_sp bool_t in_flight if in_flight int64_t rc_x = -radio_control.values RADIO_PITCH int64_t rc_y = radio_control.values RADIO_ROLL DeadBand rc_x MAX_PPRZ/20 DeadBand rc_y MAX_PPRZ/20 int32_t max_speed = SPEED_BFP_OF_REAL GUIDANCE_H_REF_MAX_SPEED rc_x = rc_x * max_speed / MAX_PPRZ rc_y = rc_y * max_speed / MAX_PPRZ int32_t psi = -stateGetNedToBodyEulers_i ->psi int32_t s_psi c_psi PPRZ_ITRIG_SIN s_psi psi PPRZ_ITRIG_COS c_psi psi speed_sp->x = int32_t int64_t c_psi * rc_x - int64_t s_psi * rc_y >> INT32_TRIG_FRAC speed_sp->y = int32_t int64_t s_psi * rc_x + int64_t c_psi * rc_y >> INT32_TRIG_FRAC else speed_sp->x = 0 speed_sp->y = 0 "
Paparazzi 285 guidance_h.c Control 4775fc5244 Added RC setpoint control in H-H mode " #ifdef GUIDANCE_H_USE_SPEED_REF struct Int32Vect2 guidance_h_speed_sp #endif #ifdef GUIDANCE_H_USE_SPEED_REF if in_flight int32_t psi s_psi c_psi rc_norm max_pprz int64_t rc_x rc_y int64_t max_speed = SPEED_BFP_OF_REAL GUIDANCE_H_REF_MAX_SPEED rc_x = int64_t radio_control.values RADIO_PITCH rc_y = int64_t radio_control.values RADIO_ROLL DeadBand rc_x MAX_PPRZ/20 DeadBand rc_y MAX_PPRZ/20 rc_norm = sqrt pow rc_x 2 + pow rc_y 2 rc_x = abs rc_x rc_y = abs rc_y max_pprz = rc_norm * MAX_PPRZ / Max rc_x rc_y rc_x = rc_x * max_speed / max_pprz rc_y = -rc_y * max_speed / max_pprz psi = stateGetNedToBodyEulers_i ->psi PPRZ_ITRIG_SIN s_psi psi PPRZ_ITRIG_COS c_psi psi guidance_h_speed_sp.x = int32_t int64_t -c_psi * rc_x + int64_t s_psi * rc_y / 1 << INT32_TRIG_FRAC guidance_h_speed_sp.y = int32_t int64_t -s_psi * rc_x - int64_t c_psi * rc_y / 1 << INT32_TRIG_FRAC else stabilization_attitude_enter #endif #if GUIDANCE_H_USE_SPEED_REF if guidance_h_mode == GUIDANCE_H_MODE_HOVER gh_update_ref_from_speed_sp guidance_h_speed_sp else #endif VECT2_COPY guidance_h_pos_sp guidance_h_pos_ref "
Paparazzi 286 guidance_h.c Control 03234de546 [rotorcraft][guidance_h] guidance_h_approx_force_by_thrust- enable/disable via settings- gets a better approximation of vertical thrust using guidance_v_thrust_coeff- vertical thrust is recomputed in guidance_h to take the latest thrust command after guidance_v was run " #include firmwares/rotorcraft/guidance/guidance_v.h #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE #endif bool_t guidance_h_approx_force_by_thrust guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST if guidance_h_approx_force_by_thrust && in_flight int32_t vertical_thrust = stabilization_cmd COMMAND_THRUST * guidance_v_thrust_coeff >> INT32_TRIG_FRAC thrust_cmd_filt = thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust / GUIDANCE_H_THRUST_CMD_FILTER + 1 "
Paparazzi 287 guidance_h.c Control 4d8d2a907d [rotorcraft] add GUIDANCE_H_APPROX_FORCE_BY_THRUSTbased on the idea of Sergei from pull request #532Still need to use only the vertical thrust instead of total thrust.Will be corrected later using the get_vertical_thrust_coeff from pull request #528 " #ifndef GUIDANCE_H_THRUST_CMD_FILTER #define GUIDANCE_H_THRUST_CMD_FILTER 10 #endif static const int32_t traj_max_bank = Max BFP_OF_REAL GUIDANCE_H_MAX_BANK INT32_ANGLE_FRAC BFP_OF_REAL RadOfDeg 40 INT32_ANGLE_FRAC #if GUIDANCE_H_APPROX_FORCE_BY_THRUST if in_flight static int32_t thrust_cmd_filt thrust_cmd_filt = thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + stabilization_cmd COMMAND_THRUST / GUIDANCE_H_THRUST_CMD_FILTER + 1 guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL atan2f guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2 thrust_cmd_filt #endif VECT2_STRIM guidance_h_cmd_earth -traj_max_bank traj_max_bank "
Paparazzi 288 guidance_h.c Non-Control 969705306b [rotorcraft] ups fix setting commands from guidance_h " stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth stabilization_attitude_set_earth_cmd_i &guidance_h_cmd_earth "
Paparazzi 289 guidance_h.c Non-Control d1a180de02 [rotorcraft] update read_rc_roll_pitch_quat " "
Paparazzi 290 guidance_h.c Control f0e5a20000 [rotorcraft] guidance_h commands in earth frame " struct Int32Vect2 guidance_h_cmd_earth int32_t guidance_h_heading_sp guidance_h_heading_sp = 0 guidance_h_heading_sp = guidance_h_rc_sp.psi stabilization_attitude_set_earth_cmd_i guidance_h_cmd_earth guidance_h_heading_sp stabilization_attitude_set_rpy_setpoint_i &sp_cmd_i guidance_h_heading_sp = nav_heading INT32_ANGLE_NORMALIZE guidance_h_heading_sp stabilization_attitude_set_earth_cmd_i guidance_h_cmd_earth guidance_h_heading_sp guidance_h_cmd_earth.x = guidance_h_cmd_earth.y = VECT2_STRIM guidance_h_cmd_earth -TRAJ_MAX_BANK TRAJ_MAX_BANK "
Paparazzi 291 guidance_h.c Non-Control c5c8772458 [rotorcraft] ups fix wrong endif " stabilization_attitude_enter stabilization_attitude_enter "
Paparazzi 292 guidance_h.c Control 07a1dc6908 [rotorcraft] by default always reset attitude stab on mode change againTo only reset (psi setpoint to current value reset ref reset integrators) the attitude stabilizationif the previous mode was not using it define NO_ATTITUDE_RESET_ON_MODE_CHANGE " #if NO_ATTITUDE_RESET_ON_MODE_CHANGE guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT #endif #if NO_ATTITUDE_RESET_ON_MODE_CHANGE guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT #endif #if NO_ATTITUDE_RESET_ON_MODE_CHANGE guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT #endif"
Paparazzi 293 guidance_h.c Control 29da4ede96 [rotorcraft] also call stabilization_attitude_enter if previous mode was GUIDANCE_H_MODE_KILL " if guidance_h_mode == GUIDANCE_H_MODE_KILL || guidance_h_mode == GUIDANCE_H_MODE_RATE || guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT if guidance_h_mode == GUIDANCE_H_MODE_KILL || guidance_h_mode == GUIDANCE_H_MODE_RATE || guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT if guidance_h_mode == GUIDANCE_H_MODE_KILL || guidance_h_mode == GUIDANCE_H_MODE_RATE || guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT "
Paparazzi 294 guidance_h.c Control c32ea43ed2 [rotorcraft] guidance_h: normalize psi sp from nav_heading " INT32_ANGLE_NORMALIZE guidance_h_command_body.psi "
Paparazzi 295 guidance_h.c Non-Control 104acd2767 [rotorcraft][guidance][stabilization] quat setpoint fixesDon t pretend that the commands from guidance_h are actually real euler angles.Compose a roll/pitch quaternion from simultaneous rotation of roll/pitch then rotate around resulting body z-axis to align the heading.This should fix the setpoint passed to the attitude stabilization if in HOVER or NAVat large angles.Only tested quickly in simulation... " struct Int32Eulers sp_cmd_i sp_cmd_i.phi = nav_roll sp_cmd_i.theta = nav_pitch sp_cmd_i.psi = stateGetNedToBodyEulers_i ->psi stabilization_attitude_set_cmd_i &sp_cmd_i stabilization_attitude_set_cmd_i &guidance_h_command_body "
Paparazzi 296 guidance_h.c Control 71efabe590 [rotorcraft] only reset psi and integrators if previous mode was not attitude " if guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || guidance_h_mode == GUIDANCE_H_MODE_RATE stabilization_attitude_enter "
Paparazzi 297 guidance_h.c Non-Control 09680b07e9 [telemetry] fix some include and remove unnecessary files ""
Paparazzi 298 guidance_h.c Non-Control 116faaeda6 [telemetry] protect telemetry code with #if DOWNLINK- also get rid of AP_DOWNLINK and FBW_DOWNLINK- still needs to make it work again (for the only remaining classix in the world...) " #if DOWNLINK #include subsystems/datalink/telemetry.h #endif #if DOWNLINK #endif"
Paparazzi 299 guidance_h.c Control 8377d60184 [telemetry] adapt rotorcraft to new telemetry system " #include mcu_periph/uart.h #include subsystems/datalink/downlink.h #include generated/periodic_telemetry.h static void send_gh void struct NedCoor_i* pos = stateGetPositionNed_i DOWNLINK_SEND_GUIDANCE_H_INT DefaultChannel DefaultDevice &guidance_h_pos_sp.x &guidance_h_pos_sp.y &guidance_h_pos_ref.x &guidance_h_pos_ref.y & pos->x & pos->y static void send_hover_loop void struct NedCoor_i* pos = stateGetPositionNed_i struct NedCoor_i* speed = stateGetSpeedNed_i struct NedCoor_i* accel = stateGetAccelNed_i DOWNLINK_SEND_HOVER_LOOP DefaultChannel DefaultDevice &guidance_h_pos_sp.x &guidance_h_pos_sp.y & pos->x & pos->y & speed->x & speed->y & accel->x & accel->y &guidance_h_pos_err.x &guidance_h_pos_err.y &guidance_h_speed_err.x &guidance_h_speed_err.y &guidance_h_pos_err_sum.x &guidance_h_pos_err_sum.y &guidance_h_nav_err.x &guidance_h_nav_err.y &guidance_h_command_earth.x &guidance_h_command_earth.y &guidance_h_command_body.phi &guidance_h_command_body.theta &guidance_h_command_body.psi static void send_href void DOWNLINK_SEND_GUIDANCE_H_REF_INT DefaultChannel DefaultDevice &guidance_h_pos_sp.x &guidance_h_pos_ref.x &guidance_h_speed_ref.x &guidance_h_accel_ref.x &guidance_h_pos_sp.y &guidance_h_pos_ref.y &guidance_h_speed_ref.y &guidance_h_accel_ref.y register_periodic_telemetry DefaultPeriodic GUIDANCE_H_INT send_gh register_periodic_telemetry DefaultPeriodic HOVER_LOOP send_hover_loop register_periodic_telemetry DefaultPeriodic GUIDANCE_H_REF send_href "
Paparazzi 300 guidance_h.c Control 8d2781e256 [rotorcraft] horizontal guidance mode switchingIf swtiching to HOVER or NAV mode:reset attitude stabilization if previous mode was not using it " static inline void reset_guidance_reference_from_current_position void if new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE if guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || guidance_h_mode == GUIDANCE_H_MODE_RATE stabilization_attitude_enter if guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || guidance_h_mode == GUIDANCE_H_MODE_RATE stabilization_attitude_enter reset_guidance_reference_from_current_position reset_guidance_reference_from_current_position "
Paparazzi 301 guidance_h.c Control 1de769b6f2 [rotorcraft] big fat attempt to clean stabilization and guidance- No more STABILIZATION_ATTITUDE_FLOAT_x in airframe file.- Hopefully seamlessly switch between int and float.- Also make it possible to not use a reference at all e.g. for passthrough " static inline void reset_reference_from_current_position void VECT2_COPY guidance_h_pos_ref *stateGetPositionNed_i VECT2_COPY guidance_h_speed_ref *stateGetSpeedNed_i INT_VECT2_ZERO guidance_h_accel_ref gh_set_ref guidance_h_pos_ref guidance_h_speed_ref guidance_h_accel_ref INT_VECT2_ZERO guidance_h_pos_err_sum if !in_flight guidance_h_hover_enter if !in_flight guidance_h_nav_enter struct Int32Eulers sp_euler_i sp_euler_i.phi = nav_roll sp_euler_i.theta = nav_pitch sp_euler_i.psi = stateGetNedToBodyEulers_i ->psi stabilization_attitude_set_from_eulers_i &sp_euler_i stabilization_attitude_set_from_eulers_i &guidance_h_command_body reset_reference_from_current_position guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i ->psi reset_reference_from_current_position nav_heading = stateGetNedToBodyEulers_i ->psi "
Paparazzi 302 guidance_h.c Non-Control 3710206de3 Added possibility to switch sticks " #if SWITCH_STICKS_FOR_RATE_CONTROL stabilization_rate_read_rc_switched_sticks #else #endif"
Paparazzi 303 guidance_h.c Control 09c97dd6c4 [rotorcraft] TRANSITION_MAX_OFFSET in radians float fixes " const int32_t max_offset = ANGLE_BFP_OF_REAL TRANSITION_MAX_OFFSET "
Paparazzi 304 guidance_h.c Control 40248363dd [rotorcraft] Add forward mode for transitioning vehicles " int32_t transition_percentage int32_t transition_theta_offset static inline void transition_run void transition_percentage = 0 transition_theta_offset = 0 if new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE transition_percentage = 0 transition_theta_offset = 0 case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_FORWARD: if transition_percentage < 100<
Paparazzi 305 guidance_h.c Non-Control adca9867d1 [rotorcraft] silence warning in guidance " static void guidance_h_update_reference void static void guidance_h_update_reference void "
Paparazzi 306 guidance_h.c Control 370100b83e [rotorcraft] use horizontal guidance ref by defaultAlso use the horizontal guidance reference for HOVER mode.Using the reference can be temporarly disabled via settings.The reference is then still computed to stay consistent but not used. " #if GUIDANCE_H_PGAIN < 0 || \ GUIDANCE_H_DGAIN < 0 || \ GUIDANCE_H_IGAIN < 0 #error ALL control gains have to be positive!!! #endif #ifndef GUIDANCE_H_AGAIN #define GUIDANCE_H_AGAIN 0 #else #if GUIDANCE_H_AGAIN < 0 #error ALL control gains have to be positive!!! #endif #endif #ifndef GUIDANCE_H_MAX_BANK #define GUIDANCE_H_MAX_BANK RadOfDeg 20 #endif PRINT_CONFIG_VAR GUIDANCE_H_USE_REF bool_t guidance_h_use_ref static void guidance_h_update_reference static void guidance_h_traj_run bool_t in_flight static void guidance_h_hover_enter void static void guidance_h_nav_enter void gh_set_ref _pos _speed _accel \ guidance_h_use_ref = GUIDANCE_H_USE_REF guidance_h_update_reference guidance_h_update_reference static void guidance_h_update_reference #if GUIDANCE_H_USE_REF gh_update_ref_from_pos_sp guidance_h_pos_sp #endif if guidance_h_use_ref static void guidance_h_traj_run bool_t in_flight static void guidance_h_hover_enter void static void guidance_h_nav_enter void "
Paparazzi 307 guidance_h.c Non-Control cdbfcb47d7 [guidance] split h_ref and v_ref into h and c files " gh_set_ref _pos _speed _accel \ gh_update_ref_from_pos_sp guidance_h_pos_sp INT32_VECT2_RSHIFT guidance_h_pos_ref gh_pos_ref GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT guidance_h_speed_ref gh_speed_ref INT32_SPEED_FRAC - GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT guidance_h_accel_ref gh_accel_ref INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC "
Paparazzi 308 guidance_h.c Control 68d07e6128 [rotorcraft] horizontal guidance fixedpoint fixesSignificantly reduce the quantization error of the horizontal guidance cmds.This should result in nicer position hold and better trajectory tracking.Verified in NPS sim. " guidance_h_pgain * guidance_h_pos_err.x >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.x >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 + guidance_h_igain * guidance_h_pos_err_sum.x >> 12 >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_again * guidance_h_accel_ref.x >> 8 guidance_h_pgain * guidance_h_pos_err.y >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.y >> 2 >> INT32_SPEED_FRAC - GH_GAIN_SCALE - 2 + guidance_h_igain * guidance_h_pos_err_sum.y >> 12 >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_again * guidance_h_accel_ref.y >> 8 "
Paparazzi 309 guidance_h.c Control 3a7bba4a6e [rotorcraft] care free mode- still not implemented for stabilization_euler- move read_rc_setpoint_* functions to c file- add GUIDANCE_H_MODE_CARE_FREE mode- add stabilization float_euler subsystem- use normal stabilization subsystem in NPS instead of hardcoded euler " #if GUIDANCE_H_PGAIN < 0 || \ GUIDANCE_H_DGAIN < 0 || \ b2_gh_set_ref _pos _speed _accel \ VECT2_COPY guidance_h_pos_ref _pos \ VECT2_COPY guidance_h_speed_ref _speed \ VECT2_COPY guidance_h_accel_ref _accel \ case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_enter break case GUIDANCE_H_MODE_RATE: stabilization_rate_enter break case GUIDANCE_H_MODE_CARE_FREE: stabilization_attitude_reset_care_free_heading case GUIDANCE_H_MODE_ATTITUDE: stabilization_attitude_enter break case GUIDANCE_H_MODE_HOVER: guidance_h_hover_enter break case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter break default: break case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_read_rc break case GUIDANCE_H_MODE_RATE: stabilization_rate_read_rc break case GUIDANCE_H_MODE_CARE_FREE: case GUIDANCE_H_MODE_ATTITUDE: stabilization_attitude_read_rc in_flight break case GUIDANCE_H_MODE_HOVER: break case GUIDANCE_H_MODE_NAV: if radio_control.status == RC_OK stabilization_attitude_read_rc_setpoint_eulers &guidance_h_rc_sp in_flight else INT_EULERS_ZERO guidance_h_rc_sp break default: break case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_run in_flight break case GUIDANCE_H_MODE_RATE: stabilization_rate_run in_flight break case GUIDANCE_H_MODE_CARE_FREE: case GUIDANCE_H_MODE_ATTITUDE: stabilization_attitude_run in_flight break case GUIDANCE_H_MODE_HOVER: guidance_h_update_reference FALSE guidance_h_command_body.psi = guidance_h_rc_sp.psi guidance_h_traj_run in_flight stabilization_attitude_run in_flight break case GUIDANCE_H_MODE_NAV: default: break - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y >> INT32_TRIG_FRAC "
Paparazzi 310 guidance_h.c Non-Control ecae43d551 [doxygen] only doxygen updates " /** @file firmwares/rotorcraft/guidance/guidance_h.c"
Paparazzi 311 guidance_h.c Non-Control be7daca9ae [rotorcraft] use american spelling: STABILIZATION instead of STABILISATION everywhere " #ifdef STABILIZATION_ATTITUDE_TYPE_QUAT #ifdef STABILIZATION_ATTITUDE_TYPE_QUAT #endif "
Paparazzi 312 guidance_h.c Non-Control 472ac96fbf [guidance] convert rotorcraft guidance to new state interface " #include state.h stab_att_sp_euler.psi = stateGetNedToBodyEulers_i ->psi int32_t psi = stateGetNedToBodyEulers_i ->psi PPRZ_ITRIG_SIN s_psi psi PPRZ_ITRIG_COS c_psi psi guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i ->psi nav_heading = stateGetNedToBodyEulers_i ->psi "
Paparazzi 313 guidance_h.c Non-Control f5054fcb6b [rotorcraft] use new state interface instead of ins in guidance " VECT2_DIFF guidance_h_pos_err guidance_h_pos_ref *stateGetPositionNed_i VECT2_DIFF guidance_h_speed_err guidance_h_speed_ref *stateGetSpeedNed_i VECT2_COPY guidance_h_pos_sp *stateGetPositionNed_i VECT2_COPY pos *stateGetPositionNed_i VECT2_COPY speed *stateGetSpeedNed_i "
Paparazzi 314 guidance_h.c Control 448e91c306 [rotorcraft guidance] get rid of guidance_h_psi_sp* directly set final guidance_h_command_body.psi * in HOVER directly from RC psi setpoint * in NAV directly from nav_heading* This should also fix a bug with changing psi setpoint when switching between HOVER and NAV mode. " guidance_h_command_body.psi = guidance_h_rc_sp.psi stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi guidance_h_command_body.psi = nav_heading guidance_h_command_body.phi += guidance_h_rc_sp.phi guidance_h_command_body.theta += guidance_h_rc_sp.theta nav_heading = ahrs.ltp_to_body_euler.psi "
Paparazzi 315 guidance_h.c Non-Control 9782d0a12b [rotorcraft guidance] minor cleanup when using guidance_h ref " #if GUIDANCE_H_USE_REF #else guidance_h_update_reference FALSE #endif"
Paparazzi 316 guidance_h.c Control 4e570aeb47 rotorcraft guidance_h: make max bank angle in nav configurable in airframe file default is still 20deg " #ifndef GUIDANCE_H_MAX_BANK #define GUIDANCE_H_MAX_BANK RadOfDeg 20 #endif #define TRAJ_MAX_BANK BFP_OF_REAL GUIDANCE_H_MAX_BANK INT32_ANGLE_FRAC "
Paparazzi 317 guidance_h.c Control 9be470a908 rotorcraft guidance horizontal: divide gain scale for accel (feed-forward) gain by 256 should be same as in master again " guidance_h_again * guidance_h_accel_ref.x >> 8 guidance_h_again * guidance_h_accel_ref.y >> 8 "
Paparazzi 318 guidance_h.c Non-Control 9f980d6551 rotorcraft horizontal guidance: fix earth.y command rshift instead of lshift " guidance_h_pgain * guidance_h_pos_err.y >> INT32_POS_FRAC - GH_GAIN_SCALE +"
Paparazzi 319 guidance_h.c Non-Control f1183faa7a get rid of some unused arg warnings " if use_ref "
Paparazzi 320 guidance_h.c Non-Control 67743b5150 rotorcraft guidance: removed ngain since tracking algorithm was removed ""
Paparazzi 321 guidance_h.c Control ba8aa325af rotorcraft horizontal guidance:* removed tracking algorithm (no integral part) from nav_run * was used if position error > HOLD_DISTANCE * but was not working properly* use the same traj_run for both hover and nav modes* in hover set the reference speed and accel to zero* in nav use compute real reference if GUIDANCE_H_USE_REF just like before " static inline void guidance_h_update_reference bool_t use_ref static inline void guidance_h_traj_run bool_t in_flight guidance_h_update_reference FALSE guidance_h_traj_run in_flight guidance_h_update_reference TRUE guidance_h_traj_run in_flight static inline void guidance_h_update_reference bool_t use_ref if use_ref b2_gh_update_ref_from_pos_sp guidance_h_pos_sp INT32_VECT2_RSHIFT guidance_h_pos_ref b2_gh_pos_ref B2_GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT guidance_h_speed_ref b2_gh_speed_ref INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT guidance_h_accel_ref b2_gh_accel_ref INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC else VECT2_COPY guidance_h_pos_ref guidance_h_pos_sp INT_VECT2_ZERO guidance_h_speed_ref INT_VECT2_ZERO guidance_h_accel_ref #define MAX_POS_ERR POS_BFP_OF_REAL 16. #define MAX_SPEED_ERR SPEED_BFP_OF_REAL 16. #define MAX_POS_ERR_SUM int32_t MAX_POS_ERR << 12 #define GH_GAIN_SCALE 2 #define TRAJ_MAX_BANK BFP_OF_REAL 0.35 INT32_ANGLE_FRAC static inline void guidance_h_traj_run bool_t in_flight if in_flight else INT_VECT2_ZERO guidance_h_pos_err_sum guidance_h_pgain * guidance_h_pos_err.x >> INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.x >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h_igain * guidance_h_pos_err_sum.x >> 12 + INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_pgain * guidance_h_pos_err.y << INT32_POS_FRAC - GH_GAIN_SCALE + guidance_h_dgain * guidance_h_speed_err.y >> INT32_SPEED_FRAC - GH_GAIN_SCALE + guidance_h_igain * guidance_h_pos_err_sum.y >> 12 + INT32_POS_FRAC - GH_GAIN_SCALE + VECT2_STRIM guidance_h_command_earth -TRAJ_MAX_BANK TRAJ_MAX_BANK static inline void guidance_h_hover_enter void static inline void guidance_h_nav_enter void "
Paparazzi 322 guidance_h.c Non-Control 577b52a016 rotorcraft guidance and stabilization:* moved reading of RC setpoint to separate header file* fix and clean up resetting of psi reference* more minor cleanup " #include firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h #include subsystems/ahrs.h stabilization_attitude_read_rc_setpoint_eulers &guidance_h_rc_sp in_flight stabilization_attitude_read_rc_setpoint_eulers &guidance_h_rc_sp in_flight guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi reset_psi_ref_from_body reset_psi_ref_from_body guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi guidance_h_psi_sp = ahrs.ltp_to_body_euler.psi nav_heading = guidance_h_psi_sp "
Paparazzi 323 guidance_h.c Control 812b6b32bd rotorcraft guidance and stabilization: start changeing all euler setpoints to INT32_ANGLE_FRAConly reference eulers with REF_ANGLE_FRAC " #ifdef STABILISATION_ATTITUDE_TYPE_QUAT guidance_h_psi_sp = nav_heading >> INT32_TRIG_FRAC + 6 >> INT32_TRIG_FRAC + 6 guidance_h_command_body.psi = guidance_h_psi_sp EULERS_ADD guidance_h_command_body guidance_h_rc_sp INT32_ANGLE_NORMALIZE guidance_h_command_body.psi EULERS_COPY stab_att_sp_euler guidance_h_command_body #define NAV_MAX_BANK BFP_OF_REAL 0.35 INT32_ANGLE_FRAC - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y >> INT32_TRIG_FRAC - c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y >> INT32_TRIG_FRAC guidance_h_command_body.psi = guidance_h_psi_sp EULERS_ADD guidance_h_command_body guidance_h_rc_sp INT32_ANGLE_NORMALIZE guidance_h_command_body.psi EULERS_COPY stab_att_sp_euler guidance_h_command_body "
Paparazzi 324 guidance_h.c Non-Control acdd03e3e0 Merge branch dev into 4.0_beta* preliminary fix for rotorcraft guidance when fixed-point quaternion stabilization is used ""
Paparazzi 325 guidance_h.c Control f8fd4f8799 rotorcraft guidance: hopefully fixed the fixedpoint resolution for attitude setpoint when using the quaternion stabilization* still needs to be cleaned up properly it s bad that the stab_att_sp_euler has a different fixedpoint fraction for quat than for euler " stab_att_sp_euler.phi = nav_roll stab_att_sp_euler.theta = nav_pitch INT32_QUAT_WRAP_SHORTEST stab_att_sp_quat #else stab_att_sp_euler.phi = nav_roll << REF_ANGLE_FRAC - INT32_ANGLE_FRAC stab_att_sp_euler.theta = nav_pitch << REF_ANGLE_FRAC - INT32_ANGLE_FRAC #ifdef STABILISATION_ATTITUDE_TYPE_INT INT32_EULERS_LSHIFT stab_att_sp_euler guidance_h_command_body REF_ANGLE_FRAC - INT32_ANGLE_FRAC INT32_QUAT_WRAP_SHORTEST stab_att_sp_quat #else EULERS_COPY stab_att_sp_euler guidance_h_command_body #endif #else EULERS_COPY stab_att_sp_euler guidance_h_command_body #endif #ifdef STABILISATION_ATTITUDE_TYPE_INT #endif INT32_EULERS_LSHIFT stab_att_sp_euler guidance_h_command_body REF_ANGLE_FRAC - INT32_ANGLE_FRAC INT32_QUAT_WRAP_SHORTEST stab_att_sp_quat #else EULERS_COPY stab_att_sp_euler guidance_h_command_body #endif "
Paparazzi 326 guidance_h.c Non-Control c840489326 Merge branch dev into 4.0_beta* updated tests* set quaternion setpoint in hover and attitude nav ""
Paparazzi 327 guidance_h.c Control 8a95ddd256 rotorcraft guidance: if quaternion stabilization is used compute quat setpoint from eulers in hover and attitude nav as well " #ifdef STABILISATION_ATTITUDE_TYPE_QUAT INT32_QUAT_OF_EULERS stab_att_sp_quat stab_att_sp_euler #endif #ifdef STABILISATION_ATTITUDE_TYPE_QUAT INT32_QUAT_OF_EULERS stab_att_sp_quat stab_att_sp_euler #endif"
Paparazzi 328 guidance_h.c Non-Control fe7c72c741 rotorcraft guidance and stabilization: only documentation and cosmetics " "
Paparazzi 329 guidance_h.c Non-Control 596fee5011 Merge branch dev into 4.0_beta ""
Paparazzi 330 guidance_h.c Control 58b842d472 rotorcraft guidance_h: only compute quaternion setpoint if we actually have a quaternion versionadded a STABILISATION_ATTITUDE_TYPE_QUAT define... maybe not the best way to go but works for now... " #ifdef STABILISATION_ATTITUDE_TYPE_QUAT #endif"
Paparazzi 331 guidance_h.c Control c2bc03e6fd rotorcraft guidance_h: also compute quaternion setpoint from euler setpoint " INT32_QUAT_OF_EULERS stab_att_sp_quat stab_att_sp_euler "
Paparazzi 332 guidance_h.c Non-Control 2044e9c081 Merge branch dev into 4.0_beta* some minor systime fixes* rc_direct helicopter safety pilot mode* nav_catapult* add an option to only show FP of the selected aircraft ""
Paparazzi 333 guidance_h.c Non-Control f415f44be9 Merge branch master into dev* nav_catapult* more fixedwing tuning params* helicopter safety pilot stuff stabilization_none ""
Paparazzi 334 guidance_h.c Control 7f1a7e4294 Helicopter Safety Pilot Code " case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_enter break case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_read_rc break case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_run in_flight break "
Paparazzi 335 guidance_h.c Non-Control a984fb6eb7 Merge branch dev into 4.0_betacontains fixes for:* stabilization int_quat now works correctly for quads and final command always works (even if pitch command is first command in airframe file)* added sys_time_usleep implementation for stm32 this fixes xbee_api mode again ""
Paparazzi 336 guidance_h.c Non-Control 6e1072abea rotorcraft stabilization int:* moved common defines to attitude_ref_int.h* replaced STABILIZATION_ATTITUDE_READ_RC macro with stabilization_attitude_read_rc_ref static inline function* removed unused stab_att_sp_vi_euler and stab_att_sp_rc_euler from attitude_ref_int.h " stabilization_attitude_read_rc_ref &guidance_h_rc_sp in_flight stabilization_attitude_read_rc_ref &guidance_h_rc_sp in_flight "
Paparazzi 337 guidance_h.c Control df76dc8fbe rotorcraft gains: do not use ABS anymore warn instead if gains are negative " #if GUIDANCE_H_PGAIN < 0 || \ GUIDANCE_H_DGAIN < 0 || \ GUIDANCE_H_IGAIN < 0 #warning ALL control gains are now positive!!! #endif #else #if GUIDANCE_H_NGAIN < 0 #warning ALL control gains are now positive!!! #endif #else #if GUIDANCE_H_AGAIN < 0 #warning ALL control gains are now positive!!! #endif guidance_h_pgain = GUIDANCE_H_PGAIN guidance_h_igain = GUIDANCE_H_IGAIN guidance_h_dgain = GUIDANCE_H_DGAIN guidance_h_ngain = GUIDANCE_H_NGAIN guidance_h_again = GUIDANCE_H_AGAIN "
Paparazzi 338 guidance_h.c Non-Control f145bc74c9 rotorcraft: fix guidance_h_speed_err " guidance_h_speed_err.x = -ins_ltp_speed.x guidance_h_speed_err.y = -ins_ltp_speed.y "
Paparazzi 339 guidance_h.c Non-Control ea47766125 Merge branch positive_control_gains into rotorcraft_positive_control_gainsConflicts:sw/airborne/math/pprz_algebra.h ""
Paparazzi 340 guidance_h.c Non-Control 00f8eec9b1 use #if USE_x instead of #ifdef USE_xThen you can also define e.g. -DUSE_GPS=0 and it will work as expected(disable it just like as if it would be undefined)-DUSE_GPS (without a value) has the same effect as -DUSE_GPS=1If you need to define it in the code use #define USE_GPS 1Please always use #if instead of #ifdef if it is a flag to enable/disable somethingand it will always evaluate as an integer arithmetic expression. " #if GUIDANCE_H_USE_REF #if GUIDANCE_H_USE_REF"
Paparazzi 341 guidance_h.c Control b78989045c changed rotorcraft guidance_h gains to positive " guidance_h_pgain = ABS GUIDANCE_H_PGAIN guidance_h_igain = ABS GUIDANCE_H_IGAIN guidance_h_dgain = ABS GUIDANCE_H_DGAIN guidance_h_ngain = ABS GUIDANCE_H_NGAIN guidance_h_again = ABS GUIDANCE_H_AGAIN VECT2_DIFF guidance_h_pos_err guidance_h_pos_sp ins_ltp_pos VECT2_COPY guidance_h_speed_err -ins_ltp_speed VECT2_DIFF guidance_h_pos_err guidance_h_pos_ref ins_ltp_pos VECT2_DIFF guidance_h_speed_err guidance_h_speed_ref ins_ltp_speed vect_prod * ins_ltp_speed.y vect_prod * -ins_ltp_speed.x "
Paparazzi 342 guidance_h.c Control c8572b2481 reset guidance_h_nav_err to zero if within HOLD_DISTANCE " INT_VECT2_ZERO guidance_h_nav_err "
Paparazzi 343 guidance_h.c Control 6b6b32a715 rotorcraft guidance_h: set the same gain multipliers in hover as in nav.With this the hover (stay) behaviour should be the same in AP_HOVER and in nav (without using the ref).This means that if you tuned the p-gain and i-gain in AP_MODE_HOVER_x before you will have to divide the old p-gain by 2 and the old i-gain by 4. " guidance_h_command_earth.x = guidance_h_pgain * guidance_h_pos_err.x << 10 - INT32_POS_FRAC + guidance_h_dgain * guidance_h_speed_err.x >> INT32_SPEED_FRAC - 10 + guidance_h_igain * guidance_h_pos_err_sum.x >> 12 + INT32_POS_FRAC - 10 guidance_h_command_earth.y = guidance_h_pgain * guidance_h_pos_err.y << 10 - INT32_POS_FRAC + guidance_h_dgain * guidance_h_speed_err.y >> INT32_SPEED_FRAC - 10 + guidance_h_igain * guidance_h_pos_err_sum.y >> 12 + INT32_POS_FRAC - 10 "
Paparazzi 344 guidance_h.c Non-Control 7fd01bcbe2 put generated headers in a seperate generated dir and specifically include generated/xxx.h " #include generated/airframe.h"
Paparazzi 345 guidance_h.c Non-Control a6cc31e36a Make static inlines also always_inline to get rid of a warning in gcc 4.5. We want to inline these functions at any cost anyways even if this will make the code grow. " __attribute__ always_inline static inline void guidance_h_hover_run void __attribute__ always_inline static inline void guidance_h_nav_run bool_t in_flight __attribute__ always_inline static inline void guidance_h_hover_enter void __attribute__ always_inline static inline void guidance_h_nav_enter void "
Paparazzi 346 guidance_h.c Non-Control 8620ecb82b moved ins to general subsystem " #include subsystems/ins.h"
Paparazzi 347 guidance_h.c Non-Control 08716bbd8d use double quotes for subsystems and firmware angle brackets only for external includes " #include firmwares/rotorcraft/guidance/guidance_h.h #include subsystems/ahrs.h #include firmwares/rotorcraft/stabilization.h #include firmwares/rotorcraft/ins.h #include firmwares/rotorcraft/navigation.h"
Paparazzi 348 guidance_h.c Non-Control 8bb77a3c3d start fixing rc for rotorcraft " if radio_control.status == RC_OK "
Paparazzi 349 guidance_h.c Non-Control 79a927adfb moved ahrs to general subsystems dir " #include "
Paparazzi 350 guidance_h.c Non-Control 35830e36fd rename booz_stab_att_* to stab_att_* " stab_att_sp_euler.phi = nav_roll << REF_ANGLE_FRAC - INT32_ANGLE_FRAC stab_att_sp_euler.theta = nav_pitch << REF_ANGLE_FRAC - INT32_ANGLE_FRAC EULERS_COPY stab_att_sp_euler guidance_h_command_body EULERS_COPY stab_att_sp_euler guidance_h_command_body "
Paparazzi 351 guidance_h.c Non-Control a769a5ab7d some temporary fix for booz_fms removal ""
Paparazzi 352 guidance_h.c Non-Control 16b3b0cab5 move and rename booz2_navigation to rotorcraft navigation " #include INT32_VECT2_NED_OF_ENU guidance_h_pos_sp navigation_carrot INT32_VECT2_NED_OF_ENU guidance_h_pos_sp navigation_carrot "
Paparazzi 353 guidance_h.c Non-Control 963450f6a0 rename booz_stabilization to stabilization " #include stabilization_rate_enter stabilization_attitude_enter stabilization_rate_read_rc stabilization_attitude_read_rc in_flight STABILIZATION_ATTITUDE_READ_RC guidance_h_rc_sp in_flight STABILIZATION_ATTITUDE_READ_RC guidance_h_rc_sp in_flight stabilization_rate_run in_flight stabilization_attitude_run in_flight stabilization_attitude_run in_flight stabilization_attitude_run in_flight STABILIZATION_ATTITUDE_RESET_PSI_REF guidance_h_rc_sp STABILIZATION_ATTITUDE_RESET_PSI_REF tmp_sp "
Paparazzi 354 guidance_h.c Non-Control 1cb5c50581 use full ahrs include path " #include "
Paparazzi 355 guidance_h.c Non-Control 21de0f6db2 fix guidance includes to use full path " #include "
Paparazzi 356 guidance_h.c Non-Control a21dd8b7d8 fix ins.h includes " #include "
Paparazzi 357 guidance_h.c Non-Control 4391630566 rename b2_gv/B2_GV to gv/GV " #include guidance/guidance_h.h #define GuidanceHSetRef _pos _speed _accel \ GuidanceHSetRef pos speed zero "
Paparazzi 358 guidance_h.c Non-Control 6579e914c2 rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/GUIDANCE " #define GUIDANCE_H_C #include guidance_h.h uint8_t guidance_h_mode struct Int32Vect2 guidance_h_pos_sp int32_t guidance_h_psi_sp struct Int32Vect2 guidance_h_pos_ref struct Int32Vect2 guidance_h_speed_ref struct Int32Vect2 guidance_h_accel_ref struct Int32Vect2 guidance_h_pos_err struct Int32Vect2 guidance_h_speed_err struct Int32Vect2 guidance_h_pos_err_sum struct Int32Vect2 guidance_h_nav_err struct Int32Eulers guidance_h_rc_sp struct Int32Vect2 guidance_h_command_earth struct Int32Vect2 guidance_h_stick_earth_sp struct Int32Eulers guidance_h_command_body int32_t guidance_h_pgain int32_t guidance_h_dgain int32_t guidance_h_igain int32_t guidance_h_ngain int32_t guidance_h_again #ifndef GUIDANCE_H_NGAIN #define GUIDANCE_H_NGAIN 0 #ifndef GUIDANCE_H_AGAIN #define GUIDANCE_H_AGAIN 0 static inline void guidance_h_hover_run void static inline void guidance_h_nav_run bool_t in_flight static inline void guidance_h_hover_enter void static inline void guidance_h_nav_enter void VECT2_COPY guidance_h_pos_ref _pos \ VECT2_COPY guidance_h_speed_ref _speed \ VECT2_COPY guidance_h_accel_ref _accel \ void guidance_h_init void guidance_h_mode = GUIDANCE_H_MODE_KILL guidance_h_psi_sp = 0 INT_VECT2_ZERO guidance_h_pos_sp INT_VECT2_ZERO guidance_h_pos_err_sum INT_EULERS_ZERO guidance_h_rc_sp INT_EULERS_ZERO guidance_h_command_body guidance_h_pgain = GUIDANCE_H_PGAIN guidance_h_igain = GUIDANCE_H_IGAIN guidance_h_dgain = GUIDANCE_H_DGAIN guidance_h_ngain = GUIDANCE_H_NGAIN guidance_h_again = GUIDANCE_H_AGAIN void guidance_h_mode_changed uint8_t new_mode if new_mode == guidance_h_mode switch guidance_h_mode case GUIDANCE_H_MODE_RATE: case GUIDANCE_H_MODE_ATTITUDE: case GUIDANCE_H_MODE_HOVER: guidance_h_hover_enter case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter guidance_h_mode = new_mode void guidance_h_read_rc bool_t in_flight switch guidance_h_mode case GUIDANCE_H_MODE_RATE: case GUIDANCE_H_MODE_ATTITUDE: case GUIDANCE_H_MODE_HOVER: BOOZ_STABILIZATION_ATTITUDE_READ_RC guidance_h_rc_sp in_flight case GUIDANCE_H_MODE_NAV: BOOZ_STABILIZATION_ATTITUDE_READ_RC guidance_h_rc_sp in_flight guidance_h_rc_sp.psi = 0 INT_EULERS_ZERO guidance_h_rc_sp void guidance_h_run bool_t in_flight switch guidance_h_mode case GUIDANCE_H_MODE_RATE: case GUIDANCE_H_MODE_ATTITUDE: case GUIDANCE_H_MODE_HOVER: guidance_h_hover_run case GUIDANCE_H_MODE_NAV: if !in_flight guidance_h_nav_enter INT32_VECT2_NED_OF_ENU guidance_h_pos_sp booz2_navigation_carrot #ifdef GUIDANCE_H_USE_REF b2_gh_update_ref_from_pos_sp guidance_h_pos_sp guidance_h_psi_sp = nav_heading << REF_ANGLE_FRAC - INT32_ANGLE_FRAC guidance_h_nav_run in_flight static inline void guidance_h_hover_run void VECT2_DIFF guidance_h_pos_err ins_ltp_pos guidance_h_pos_sp VECT2_STRIM guidance_h_pos_err -MAX_POS_ERR MAX_POS_ERR VECT2_COPY guidance_h_speed_err ins_ltp_speed VECT2_STRIM guidance_h_speed_err -MAX_SPEED_ERR MAX_SPEED_ERR VECT2_ADD guidance_h_pos_err_sum guidance_h_pos_err VECT2_STRIM guidance_h_pos_err_sum -MAX_POS_ERR_SUM MAX_POS_ERR_SUM guidance_h_command_earth.x = guidance_h_pgain<<1 * guidance_h_pos_err.x + guidance_h_dgain * guidance_h_speed_err.x>>9 + guidance_h_igain * guidance_h_pos_err_sum.x >> 12 guidance_h_command_earth.y = guidance_h_pgain<<1 * guidance_h_pos_err.y + guidance_h_dgain * guidance_h_speed_err.y>>9 + guidance_h_igain * guidance_h_pos_err_sum.y >> 12 VECT2_STRIM guidance_h_command_earth -MAX_BANK MAX_BANK guidance_h_command_body.phi = - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y guidance_h_command_body.theta = - c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y guidance_h_command_body.phi += guidance_h_rc_sp.phi guidance_h_command_body.theta += guidance_h_rc_sp.theta guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi ANGLE_REF_NORMALIZE guidance_h_command_body.psi EULERS_COPY booz_stab_att_sp_euler guidance_h_command_body static inline void guidance_h_nav_run bool_t in_flight #ifdef GUIDANCE_H_USE_REF INT32_VECT2_RSHIFT guidance_h_pos_ref b2_gh_pos_ref B2_GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT guidance_h_speed_ref b2_gh_speed_ref INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT guidance_h_accel_ref b2_gh_accel_ref INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC VECT2_COPY guidance_h_pos_ref guidance_h_pos_sp INT_VECT2_ZERO guidance_h_speed_ref INT_VECT2_ZERO guidance_h_accel_ref VECT2_DIFF guidance_h_pos_err ins_ltp_pos guidance_h_pos_ref VECT2_STRIM guidance_h_pos_err -MAX_POS_ERR MAX_POS_ERR VECT2_DIFF guidance_h_speed_err ins_ltp_speed guidance_h_speed_ref VECT2_STRIM guidance_h_speed_err -MAX_SPEED_ERR MAX_SPEED_ERR INT32_VECT2_NORM dist guidance_h_pos_err VECT2_ADD guidance_h_pos_err_sum guidance_h_pos_err VECT2_STRIM guidance_h_pos_err_sum -MAX_POS_ERR_SUM MAX_POS_ERR_SUM int32_t scal_prod = ins_ltp_speed.x * guidance_h_pos_err.x + ins_ltp_speed.y * guidance_h_pos_err.y vect_prod = ins_ltp_speed.x * guidance_h_pos_err.y >> INT32_POS_FRAC + INT32_SPEED_FRAC - 10 - ins_ltp_speed.y * guidance_h_pos_err.x >> INT32_POS_FRAC + INT32_SPEED_FRAC - 10 VECT2_ASSIGN guidance_h_nav_err VECT2_SDIV guidance_h_nav_err guidance_h_nav_err dist*dist if !in_flight INT_VECT2_ZERO guidance_h_pos_err_sum guidance_h_command_earth.x = guidance_h_pgain * guidance_h_pos_err.x << 10 - INT32_POS_FRAC + guidance_h_dgain * guidance_h_speed_err.x >> INT32_SPEED_FRAC - 10 + guidance_h_igain * guidance_h_pos_err_sum.x >> 12 + INT32_POS_FRAC - 10 + guidance_h_ngain * guidance_h_nav_err.x + guidance_h_again * guidance_h_accel_ref.x guidance_h_command_earth.y = guidance_h_pgain * guidance_h_pos_err.y << 10 - INT32_POS_FRAC + guidance_h_dgain * guidance_h_speed_err.y >> INT32_SPEED_FRAC - 10 + guidance_h_igain * guidance_h_pos_err_sum.y >> 12 + INT32_POS_FRAC - 10 + guidance_h_ngain * guidance_h_nav_err.y + guidance_h_again * guidance_h_accel_ref.y VECT2_STRIM guidance_h_command_earth -NAV_MAX_BANK NAV_MAX_BANK INT32_VECT2_RSHIFT guidance_h_command_earth guidance_h_command_earth REF_ANGLE_FRAC - 16 guidance_h_command_body.phi = - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y >> INT32_TRIG_FRAC - REF_ANGLE_FRAC - 16 guidance_h_command_body.theta = - c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y >> INT32_TRIG_FRAC - REF_ANGLE_FRAC - 16 guidance_h_command_body.phi += guidance_h_rc_sp.phi guidance_h_command_body.theta += guidance_h_rc_sp.theta guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi ANGLE_REF_NORMALIZE guidance_h_command_body.psi EULERS_COPY booz_stab_att_sp_euler guidance_h_command_body static inline void guidance_h_hover_enter void VECT2_COPY guidance_h_pos_sp ins_ltp_pos BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF guidance_h_rc_sp INT_VECT2_ZERO guidance_h_pos_err_sum static inline void guidance_h_nav_enter void INT32_VECT2_NED_OF_ENU guidance_h_pos_sp booz2_navigation_carrot guidance_h_psi_sp = tmp_sp.psi nav_heading = guidance_h_psi_sp >> REF_ANGLE_FRAC - INT32_ANGLE_FRAC guidance_h_rc_sp.psi = 0 INT_VECT2_ZERO guidance_h_pos_err_sum "
Paparazzi 359 guidance_h.c Non-Control 0891f38a48 moving guidance rename still to be done ""
Paparazzi 360 guidance_h.c Non-Control 8bfa75a2f9 rename booz ins ""
Paparazzi 361 guidance_h.c Non-Control c2a645a7ee rename booz_ahrs to ahrs fix makefiles ""
Paparazzi 362 guidance_h.c Non-Control 0ea232ed1b more toward modular vi ""
Paparazzi 363 guidance_h.c Non-Control 2f55f8bbbc added default in switch and missing declarations ""
Paparazzi 364 guidance_h.c Control 8e808f76f9 initialized zero variable in booz2_guidance_h_nav_enter ""
Paparazzi 365 guidance_h.c Control d6b0c1d67c add integral part to rate stabilization ""
Paparazzi 366 booz2_guidance_h.c Control a083f4a7e7 add feedforward to rate stabilization " case BOOZ2_GUIDANCE_H_MODE_RATE: booz_stabilization_rate_enter break "
Paparazzi 367 booz2_guidance_h.c Non-Control 7afcdc05bb fix variable name " INT_VECT2_ZERO booz2_guidance_h_speed_ref INT_VECT2_ZERO booz2_guidance_h_accel_ref "
Paparazzi 368 booz2_guidance_h.c Control 178d4d5037 trajectory generation for horizontal guidanceused in the navigation loop " #define B2_GUIDANCE_H_C int32_t booz2_guidance_h_psi_sp struct Int32Vect2 booz2_guidance_h_pos_ref struct Int32Vect2 booz2_guidance_h_speed_ref struct Int32Vect2 booz2_guidance_h_accel_ref #define Booz2GuidanceHSetRef _pos _speed _accel \ b2_gh_set_ref _pos _speed _accel \ VECT2_COPY booz2_guidance_h_pos_ref _pos \ VECT2_COPY booz2_guidance_h_speed_ref _speed \ VECT2_COPY booz2_guidance_h_accel_ref _accel \ #ifdef B2_GUIDANCE_H_USE_REF b2_gh_update_ref_from_pos_sp booz2_guidance_h_pos_sp #endif #ifdef B2_GUIDANCE_H_USE_REF INT32_VECT2_RSHIFT booz2_guidance_h_pos_ref b2_gh_pos_ref B2_GH_POS_REF_FRAC - INT32_POS_FRAC INT32_VECT2_LSHIFT booz2_guidance_h_speed_ref b2_gh_speed_ref INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC INT32_VECT2_LSHIFT booz2_guidance_h_accel_ref b2_gh_accel_ref INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC #else VECT2_COPY booz2_guidance_h_pos_ref booz2_guidance_h_pos_sp INT_VECT2_ZERO booz2_guidance_v_speed_ref INT_VECT2_ZERO booz2_guidance_v_accel_ref #endif VECT2_DIFF booz2_guidance_h_pos_err booz_ins_ltp_pos booz2_guidance_h_pos_ref VECT2_DIFF booz2_guidance_h_speed_err booz_ins_ltp_speed booz2_guidance_h_speed_ref booz2_guidance_h_again * booz2_guidance_h_accel_ref.x booz2_guidance_h_again * booz2_guidance_h_accel_ref.y struct Int32Vect2 pos speed zero VECT2_COPY pos booz_ins_ltp_pos VECT2_COPY speed booz_ins_ltp_speed Booz2GuidanceHSetRef pos speed zero "
Paparazzi 369 booz2_guidance_h.c Non-Control c0029db233 remove trailing whitespaces " * * Boston MA 02111-1307 USA. booz2_guidance_h_igain * booz2_guidance_h_pos_err_sum.x >> 12 booz2_guidance_h_igain * booz2_guidance_h_pos_err_sum.y >> 12 PPRZ_ITRIG_SIN s_psi booz_ahrs.ltp_to_body_euler.psi PPRZ_ITRIG_COS c_psi booz_ahrs.ltp_to_body_euler.psi booz2_guidance_h_command_body.phi = booz2_guidance_h_command_body.theta = PPRZ_ITRIG_SIN s_psi booz_ahrs.ltp_to_body_euler.psi PPRZ_ITRIG_COS c_psi booz_ahrs.ltp_to_body_euler.psi booz2_guidance_h_command_body.phi = booz2_guidance_h_command_body.theta ="
Paparazzi 370 booz2_guidance_h.c Control ddf825c9a2 hysteresis on radio ok; ground detect use main event; rc not used in guidance if lost " if radio_control.status == RADIO_CONTROL_OK BOOZ_STABILIZATION_ATTITUDE_READ_RC booz2_guidance_h_rc_sp in_flight booz2_guidance_h_rc_sp.psi = 0 else INT_EULERS_ZERO booz2_guidance_h_rc_sp if !in_flight booz2_guidance_h_nav_enter "
Paparazzi 371 booz2_guidance_h.c Control 624ce9eea9 navigation function in guidance; frequency set at 16 Hz " struct Int32Vect2 booz2_guidance_h_nav_err int32_t booz2_guidance_h_ngain int32_t booz2_guidance_h_again #ifndef BOOZ2_GUIDANCE_H_NGAIN #define BOOZ2_GUIDANCE_H_NGAIN 0 #endif #ifndef BOOZ2_GUIDANCE_H_AGAIN #define BOOZ2_GUIDANCE_H_AGAIN 0 #endif static inline void booz2_guidance_h_nav_run bool_t in_flight booz2_guidance_h_ngain = BOOZ2_GUIDANCE_H_NGAIN booz2_guidance_h_again = BOOZ2_GUIDANCE_H_AGAIN if fms.enabled && fms.input.h_mode == BOOZ2_GUIDANCE_H_MODE_ATTITUDE #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT booz_stab_att_sp_euler.phi = nav_roll << REF_ANGLE_FRAC - INT32_ANGLE_FRAC booz_stab_att_sp_euler.theta = nav_pitch << REF_ANGLE_FRAC - INT32_ANGLE_FRAC #endif booz2_guidance_h_nav_run in_flight #define NAV_MAX_BANK BFP_OF_REAL 0.35 REF_ANGLE_FRAC #define HOLD_DISTANCE POS_BFP_OF_REAL 10. static inline void booz2_guidance_h_nav_run bool_t in_flight VECT2_DIFF booz2_guidance_h_pos_err booz_ins_ltp_pos booz2_guidance_h_pos_sp VECT2_STRIM booz2_guidance_h_pos_err -MAX_POS_ERR MAX_POS_ERR VECT2_COPY booz2_guidance_h_speed_err booz_ins_ltp_speed VECT2_STRIM booz2_guidance_h_speed_err -MAX_SPEED_ERR MAX_SPEED_ERR int32_t dist INT32_VECT2_NORM dist booz2_guidance_h_pos_err if dist < HOLD_DISTANCE VECT2_ADD booz2_guidance_h_pos_err_sum booz2_guidance_h_pos_err VECT2_STRIM booz2_guidance_h_pos_err_sum -MAX_POS_ERR_SUM MAX_POS_ERR_SUM else int32_t vect_prod = 0 int32_t scal_prod = booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.x + booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.y if scal_prod >= 0 vect_prod = booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.y >> INT32_POS_FRAC + INT32_SPEED_FRAC - 10 - booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.x >> INT32_POS_FRAC + INT32_SPEED_FRAC - 10 VECT2_ASSIGN booz2_guidance_h_nav_err vect_prod * -booz_ins_ltp_speed.y vect_prod * booz_ins_ltp_speed.x VECT2_SDIV booz2_guidance_h_nav_err booz2_guidance_h_nav_err dist*dist if !in_flight INT_VECT2_ZERO booz2_guidance_h_pos_err_sum booz2_guidance_h_command_earth.x = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.x << 10 - INT32_POS_FRAC + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.x >> INT32_SPEED_FRAC - 10 + booz2_guidance_h_igain * booz2_guidance_h_pos_err_sum.x >> 12 + INT32_POS_FRAC - 10 + booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x + booz2_guidance_h_again * booz_ins_ltp_accel.x booz2_guidance_h_command_earth.y = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.y << 10 - INT32_POS_FRAC + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.y >> INT32_SPEED_FRAC - 10 + booz2_guidance_h_igain * booz2_guidance_h_pos_err_sum.y >> 12 + INT32_POS_FRAC - 10 + booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y + booz2_guidance_h_again * booz_ins_ltp_accel.y VECT2_STRIM booz2_guidance_h_command_earth -NAV_MAX_BANK NAV_MAX_BANK INT32_VECT2_RSHIFT booz2_guidance_h_command_earth booz2_guidance_h_command_earth REF_ANGLE_FRAC - 16 int32_t s_psi c_psi PPRZ_ITRIG_SIN s_psi booz_ahrs.ltp_to_body_euler.psi PPRZ_ITRIG_COS c_psi booz_ahrs.ltp_to_body_euler.psi booz2_guidance_h_command_body.phi = - s_psi * booz2_guidance_h_command_earth.x + c_psi * booz2_guidance_h_command_earth.y >> INT32_TRIG_FRAC - REF_ANGLE_FRAC - 16 booz2_guidance_h_command_body.theta = - c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y >> INT32_TRIG_FRAC - REF_ANGLE_FRAC - 16 booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi ANGLE_REF_NORMALIZE booz2_guidance_h_command_body.psi EULERS_COPY booz_stab_att_sp_euler booz2_guidance_h_command_body "
Paparazzi 372 booz2_guidance_h.c Non-Control 2765d61d59 switching to new supervision code for n-rotors " #include booz_fms.h if fms.enabled BOOZ_STABILIZATION_ATTITUDE_ADD_SP fms.input.h_sp.attitude if fms.enabled && fms.input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER "
Paparazzi 373 booz2_guidance_h.c Control 91e2d0a346 added a floating point version of the current attitude stabilization loop and a floating point version of a quaternion based attitude stabilization loop " booz_stab_att_sp_euler.phi = 0 booz_stab_att_sp_euler.theta = 0 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT booz2_guidance_h_psi_sp = nav_heading << REF_ANGLE_FRAC - INT32_ANGLE_FRAC #endif #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT #endif EULERS_COPY booz_stab_att_sp_euler booz2_guidance_h_command_body #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT nav_heading = booz2_guidance_h_psi_sp >> REF_ANGLE_FRAC - INT32_ANGLE_FRAC #endif "
Paparazzi 374 booz2_guidance_h.c Non-Control 24ad57ca46 moving files " #include booz_stabilization.h booz_stabilization_attitude_enter booz_stabilization_rate_read_rc booz_stabilization_attitude_read_rc in_flight BOOZ_STABILIZATION_ATTITUDE_ADD_SP booz_fms_input.h_sp.attitude BOOZ_STABILIZATION_ATTITUDE_READ_RC booz2_guidance_h_rc_sp in_flight BOOZ_STABILIZATION_ATTITUDE_READ_RC booz2_guidance_h_rc_sp in_flight booz_stabilization_rate_run booz_stabilization_attitude_run in_flight booz_stabilization_attitude_run in_flight booz_stabilization_attitude_run in_flight BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF booz2_guidance_h_rc_sp BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF tmp_sp "
Paparazzi 375 booz2_guidance_h.c Control fa2d6026e6 big files move/renamereplaced CONFIG with BOARD_CONFIGremoved booz_geometry_xxxx (now replaced by math/pprz_trig_int and math/pprz_algebra_xxx )moved booz/arm7 to booz/arch/lpc21 and booz/stm32 to booz/arch/stm32 " struct Int32Vect2 booz2_guidance_h_pos_sp struct Int32Vect2 booz2_guidance_h_pos_err struct Int32Vect2 booz2_guidance_h_speed_err struct Int32Vect2 booz2_guidance_h_pos_err_sum struct Int32Eulers booz2_guidance_h_rc_sp struct Int32Vect2 booz2_guidance_h_command_earth struct Int32Vect2 booz2_guidance_h_stick_earth_sp struct Int32Eulers booz2_guidance_h_command_body INT_VECT2_ZERO booz2_guidance_h_pos_err_sum INT_EULERS_ZERO booz2_guidance_h_rc_sp INT_EULERS_ZERO booz2_guidance_h_command_body #ifdef USE_FMS #endif #ifdef USE_FMS #endif #define MAX_POS_ERR POS_BFP_OF_REAL 16. #define MAX_SPEED_ERR SPEED_BFP_OF_REAL 16. VECT2_DIFF booz2_guidance_h_pos_err booz_ins_ltp_pos booz2_guidance_h_pos_sp VECT2_STRIM booz2_guidance_h_pos_err -MAX_POS_ERR MAX_POS_ERR VECT2_COPY booz2_guidance_h_speed_err booz_ins_ltp_speed VECT2_STRIM booz2_guidance_h_speed_err -MAX_SPEED_ERR MAX_SPEED_ERR VECT2_ADD booz2_guidance_h_pos_err_sum booz2_guidance_h_pos_err VECT2_STRIM booz2_guidance_h_pos_err_sum -MAX_POS_ERR_SUM MAX_POS_ERR_SUM VECT2_STRIM booz2_guidance_h_command_earth -MAX_BANK MAX_BANK PPRZ_ITRIG_SIN s_psi booz_ahrs.ltp_to_body_euler.psi PPRZ_ITRIG_COS c_psi booz_ahrs.ltp_to_body_euler.psi >> INT32_TRIG_FRAC - 2 >> INT32_TRIG_FRAC - 2 EULERS_COPY booz_stabilization_att_sp booz2_guidance_h_command_body VECT2_COPY booz2_guidance_h_pos_sp booz_ins_ltp_pos INT_VECT2_ZERO booz2_guidance_h_pos_err_sum #ifdef USE_FMS #endif struct Int32Eulers tmp_sp INT_VECT2_ZERO booz2_guidance_h_pos_err_sum "
Paparazzi 376 booz2_guidance_h.c Control 033d6334b1 switched to new radio_control code that supports both classic ppm scheme and newer serial frames from 2.4GHz spektrum transmitters. created two makefiles to select between modes ""
Paparazzi 377 booz2_guidance_h.c Non-Control 36e44fdb8d fixing licence headers thank you Piotr " "
Paparazzi 378 booz2_guidance_h.c Control 0ae85a8598 reset heading when changing mode " static inline void booz2_guidance_h_nav_enter void case BOOZ2_GUIDANCE_H_MODE_NAV: booz2_guidance_h_nav_enter break booz2_guidance_h_rc_sp.psi = 0 static inline void booz2_guidance_h_nav_enter void INT32_VECT2_NED_OF_ENU booz2_guidance_h_pos_sp booz2_navigation_carrot struct booz_ieuler tmp_sp BOOZ2_STABILIZATION_ATTITUDE_RESET_PSI_REF tmp_sp booz2_guidance_h_psi_sp = tmp_sp.psi nav_heading = booz2_guidance_h_psi_sp >> ANGLE_REF_RES - INT32_ANGLE_FRAC booz2_guidance_h_rc_sp.psi = 0 BOOZ_IVECT2_ZERO booz2_guidance_h_pos_err_sum "
Paparazzi 379 booz2_guidance_h.c Control 596cb87b41 update navigation modes for booz2 " if horizontal_mode == HORIZONTAL_MODE_ATTITUDE booz_stabilization_att_sp.phi = 0 booz_stabilization_att_sp.theta = 0 else INT32_VECT2_NED_OF_ENU booz2_guidance_h_pos_sp booz2_navigation_carrot booz2_guidance_h_psi_sp = nav_heading << ANGLE_REF_RES - INT32_ANGLE_FRAC booz2_guidance_h_hover_run booz2_stabilization_attitude_run in_flight break #define MAX_BANK 98000 "
Paparazzi 380 booz2_guidance_h.c Control c67df28ada navigation (horizontal) for booz using the flight plans " INT32_VECT2_NED_OF_ENU booz2_guidance_h_pos_sp booz2_navigation_carrot "
Paparazzi 381 booz2_guidance_h.c Non-Control 1d7290e7ce *** empty log message *** " #include booz_ahrs.h"
Paparazzi 382 booz2_guidance_h.c Control f4818d7e2d *** empty log message *** " BOOZ_ISIN s_psi booz_ahrs.ltp_to_body_euler.psi BOOZ_ICOS c_psi booz_ahrs.ltp_to_body_euler.psi "
Paparazzi 383 booz2_guidance_h.c Control 3b4dcd5c55 big fat commit - changed INS and guidance to LTPZ " int32_t booz2_guidance_h_psi_sp struct booz_ivect2 booz2_guidance_h_speed_err booz2_guidance_h_psi_sp = 0 INT_VECT2_ZERO booz2_guidance_h_pos_sp #define MAX_POS_ERR BOOZ_POS_I_OF_F 16. #define MAX_SPEED_ERR BOOZ_SPEED_I_OF_F 16. #define MAX_POS_ERR_SUM int32_t MAX_POS_ERR << 12 BOOZ_IVECT2_DIFF booz2_guidance_h_pos_err booz_ins_ltp_pos booz2_guidance_h_pos_sp BOOZ_IVECT2_COPY booz2_guidance_h_speed_err booz_ins_ltp_speed booz2_guidance_h_command_earth.x = booz2_guidance_h_pgain<<1 * booz2_guidance_h_pos_err.x + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.x>>9 + booz2_guidance_h_command_earth.y = booz2_guidance_h_pgain<<1 * booz2_guidance_h_pos_err.y + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.y>>9 + booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi ANGLE_REF_NORMALIZE booz2_guidance_h_command_body.psi BOOZ_IVECT2_COPY booz2_guidance_h_pos_sp booz_ins_ltp_pos "
LibrePilot 5756 vtolpathfollower.c Non-Control cbedb5b53b Move Guidance to VtolPathFollower to make room for FWPathFollower * @file vtolpathfollower.c * @author The OpenPilot Team http: #include vtolpathfollower.h static xTaskHandle pathfollowerTaskHandle static void vtolPathFollowerTask void *parameters int32_t VtolPathFollowerStart xTaskCreate vtolPathFollowerTask signed char * VtolPathFollower STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &pathfollowerTaskHandle TaskMonitorAdd TASKINFO_RUNNING_PATHFOLLOWER pathfollowerTaskHandle int32_t VtolPathFollowerInitialize MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart static void vtolPathFollowerTask void *parameters
LibrePilot 5758 vtolpathfollower.c Non-Control 27d1c4be93 Extend list of airframe types for VTOL guidance systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI
LibrePilot 5759 vtolpathfollower.c Control a8bdd4a44a Coalescene PositionDesired and PathDesired into PathDesired and add a modefield to indicate how it should be interpreted. #include pathdesired.h #include positiondesired.h static PathDesiredData pathDesired static GuidanceSettingsData guidanceSettings static void SettingsUpdatedCb UAVObjEvent * ev static void updateEndpointVelocity static float bound float val float min float max PathDesiredData pathDesired GuidanceSettingsConnectCallback SettingsUpdatedCb PathDesiredConnectCallback SettingsUpdatedCb GuidanceSettingsGet &guidanceSettings PathDesiredGet &pathDesired vTaskDelayUntil &lastUpdateTime guidanceSettings.UpdatePeriod / portTICK_RATE_MS if pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT updateEndpointVelocity updateVtolDesiredAttitude else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR if pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT updateEndpointVelocity updateVtolDesiredAttitude else if pathDesired.Mode == PATHDESIRED_MODE_PATH updateVtolDesiredAttitude else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break float dT = guidanceSettings.UpdatePeriod / 1000.0f void updateEndpointVelocity float dT = guidanceSettings.UpdatePeriod / 1000.0f northError = pathDesired.End PATHDESIRED_END_NORTH - northPos eastError = pathDesired.End PATHDESIRED_END_EAST - eastPos downError = pathDesired.End PATHDESIRED_END_DOWN - downPos float dT = guidanceSettings.UpdatePeriod / 1000.0f static void SettingsUpdatedCb UAVObjEvent * ev GuidanceSettingsGet &guidanceSettings PathDesiredGet &pathDesired
LibrePilot 5762 vtolpathfollower.c Non-Control 2857b58f8e Migrate path planner from position desired to path desired
LibrePilot 5763 vtolpathfollower.c Control 4ec4edd15c When enabling position hold altitude mode take the throttle from before as the hover throttle static float throttleOffset = 0 StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired throttleOffset = stabDesired.Throttle stabDesired.Throttle = bound downCommand + throttleOffset 0 1
LibrePilot 5855 vtolpathfollower.c Control 09f25ab11b Refactorted VTOLPathFollower to fit into new scheme: optional module reports status and knows advanced FlightModes #include hwsettings.h #include pathstatus.h static bool followerEnabled = false static void updateVtolFixedAttitude float* attitude if followerEnabled xTaskCreate vtolPathFollowerTask signed char * VtolPathFollower STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &pathfollowerTaskHandle TaskMonitorAdd TASKINFO_RUNNING_PATHFOLLOWER pathfollowerTaskHandle HwSettingsInitialize uint8_t optionalModules HWSETTINGS_OPTIONALMODULES_NUMELEM HwSettingsOptionalModulesGet optionalModules if optionalModules HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER == HWSETTINGS_OPTIONALMODULES_ENABLED followerEnabled = true GuidanceSettingsInitialize NedAccelInitialize PathDesiredInitialize PathStatusInitialize VelocityDesiredInitialize else followerEnabled = false PathStatusData pathStatus PathStatusGet &pathStatus if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: updateEndpointVelocity updateVtolDesiredAttitude AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_FLYVECTOR: updatePathVelocity updateVtolDesiredAttitude AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_FIXEDATTITUDE: updateVtolFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL break default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break static void updateVtolFixedAttitude float* attitude StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.Throttle = attitude 3 stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE StabilizationDesiredSet &stabDesired
LibrePilot 5922 vtolpathfollower.c Non-Control 63cd7b063f Refactored Guidance to apropriate pathfollower in uavobjects #include vtolpathfollowersettings.h static VtolPathFollowerSettingsData vtolpathfollowerSettings VtolPathFollowerSettingsInitialize VtolPathFollowerSettingsConnectCallback SettingsUpdatedCb VtolPathFollowerSettingsGet &vtolpathfollowerSettings vTaskDelayUntil &lastUpdateTime vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_KP if total_vel > vtolpathfollowerSettings.HorizontalVelMax scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_KP + downPosIntegral -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f switch vtolpathfollowerSettings.PositionSource northPosIntegral = bound northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_KP + eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalPosPI GUIDANCESETTINGS_HORIZONTALPOSPI_KP + if total_vel > vtolpathfollowerSettings.HorizontalVelMax scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI GUIDANCESETTINGS_VERTICALPOSPI_KP + downPosIntegral -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f VtolPathFollowerSettingsData vtolpathfollowerSettings VtolPathFollowerSettingsGet &vtolpathfollowerSettings switch vtolpathfollowerSettings.VelocitySource northVelIntegral = bound northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KP + nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KD + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward eastVelIntegral = bound eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KP + nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID GUIDANCESETTINGS_HORIZONTALVELPID_KD + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID GUIDANCESETTINGS_VERTICALVELPID_KI -vtolpathfollowerSettings.VerticalVelPID GUIDANCESETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID GUIDANCESETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID GUIDANCESETTINGS_VERTICALVELPID_KP + nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID GUIDANCESETTINGS_VERTICALVELPID_KD -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch if vtolpathfollowerSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE VtolPathFollowerSettingsGet &vtolpathfollowerSettings
LibrePilot 5931 vtolpathfollower.c Non-Control 9bdfe68bf5 PathFollower: refactoring (especially fixed wing) static void updateFixedAttitude float* attitude xTaskCreate vtolPathFollowerTask signed char * PathFollower STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &pathfollowerTaskHandle if optionalModules HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER == HWSETTINGS_OPTIONALMODULES_ENABLED case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: updateFixedAttitude pathDesired.ModeParameters static void updateFixedAttitude float* attitude
LibrePilot 5936 vtolpathfollower.c Non-Control 79b717b4f7 Fixes to PathFollowers float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: northPosIntegral = bound northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: northVelIntegral = bound northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + eastVelIntegral = bound eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI -vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD if vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE
LibrePilot 5960 vtolpathfollower.c Control e6f4a40d3e path library: updated API and usage from pathfollowers added new conditions and additional user feedback in pathstatus case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1
LibrePilot 5975 vtolpathfollower.c Non-Control d071491265 removed include for positiondesired.h
LibrePilot 6012 vtolpathfollower.c Control 21eb48c58c Add a RTH flight mode and remove it from the waypoint actions as that isredundant with flying to (0 0 0) case FLIGHTSTATUS_FLIGHTMODE_RTH: if pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT updateEndpointVelocity updateVtolDesiredAttitude else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break
LibrePilot 6043 vtolpathfollower.c Control 50ed363330 Axislock as default for VtolPathFollower stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK
LibrePilot 6048 vtolpathfollower.c Control 574ab3fcaa Make the VtolPathFollower an optional moduleNote that RTH mode right now checks that throttle control is enabled but thisshould only occur if the system is a VTOL. #include hwsettings.h #include vtolpathfollowersettings.h static VtolPathFollowerSettingsData guidanceSettings static bool vtolpathfollower_enabled if vtolpathfollower_enabled xTaskCreate vtolPathFollowerTask signed char * VtolPathFollower STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &pathfollowerTaskHandle TaskMonitorAdd TASKINFO_RUNNING_PATHFOLLOWER pathfollowerTaskHandle uint8_t optionalModules HWSETTINGS_OPTIONALMODULES_NUMELEM HwSettingsOptionalModulesGet optionalModules if optionalModules HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER == HWSETTINGS_OPTIONALMODULES_ENABLED VtolPathFollowerSettingsInitialize NedAccelInitialize PathDesiredInitialize VelocityDesiredInitialize vtolpathfollower_enabled = true else vtolpathfollower_enabled = false VtolPathFollowerSettingsConnectCallback SettingsUpdatedCb VtolPathFollowerSettingsGet &guidanceSettings float error_speed = progress.error * guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP downPosIntegral = bound downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: northPosIntegral = bound northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + eastPosIntegral = bound eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT eastCommand = eastError * guidanceSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + downPosIntegral = bound downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * guidanceSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral VtolPathFollowerSettingsData guidanceSettings VtolPathFollowerSettingsGet &guidanceSettings case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: northVelIntegral = bound northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + nedAccel.North * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + eastVelIntegral = bound eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + nedAccel.East * guidanceSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + downVelIntegral = bound downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI -guidanceSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT guidanceSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * guidanceSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + nedAccel.Down * guidanceSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD if guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE VtolPathFollowerSettingsGet &guidanceSettings
LibrePilot 6052 vtolpathfollower.c Non-Control 463ad550e2 A few comments in my code and some simplication to the fixedwingfollower to getit compiling against my path planner. * @brief This module compared @ref PositionActual to @ref PathDesired * and sets @ref Stabilization. It only does this when the FlightMode field * of @ref FlightStatus is PathPlanner or RTH. * Input object: FlightStatus * Input object: PathDesired * Output object: StabilizationDesired * This module will periodically update the value of the @ref StabilizationDesired object based on * @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported * by this module. Otherwise another module e.g. @ref ManualControlCommand is expected to be * writing to @ref StabilizationDesired.
LibrePilot 6238 vtolpathfollower.c Control 36fcdcc107 Make PH use axis lock by default stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK
LibrePilot 6451 vtolpathfollower.c Non-Control 86422b8b9e VtolPathFollower: Make sure to clear the alarm when running so it doesn t show asuninintialized AlarmsClear SYSTEMALARMS_ALARM_GUIDANCE
LibrePilot 6620 vtolpathfollower.c Control 5981d9acd6 removed surplus NEDposition uavobject #include homelocation.h #define DEG2RAD F_PI/180.0f GPSPositionData gpsPosition GPSPositionGet &gpsPosition HomeLocationData homeLocation HomeLocationGet &homeLocation float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD float alt = homeLocation.Altitude float T 3 = alt+6.378137E6f cosf lat * alt+6.378137E6f -1.0f float NED 3 = T 0 * gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f * DEG2RAD T 1 * gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f * DEG2RAD T 2 * gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude northPos = NED 0 eastPos = NED 1 downPos = NED 2
LibrePilot 7345 vtolpathfollower.c Control 5ded8c6dfc VTOL path fixesFlight mode fixesPOI modeAutoLand TestsSimple stabilization Shortest way yaw fixGCS map and pathplanner fixes #include cameradesired.h #include poilearnsettings.h #include poilocation.h #include accessorydesired.h #define RAD2DEG rad rad * 180.0f/F_PI static PathStatusData pathStatus static void updatePOIBearing static void updateVtolDesiredAttitude bool yaw_attitude static void accessoryUpdated UAVObjEvent* ev CameraDesiredInitialize AccessoryDesiredInitialize PoiLearnSettingsInitialize PoiLocationInitialize AccessoryDesiredConnectCallback accessoryUpdated case FLIGHTSTATUS_FLIGHTMODE_LAND: updateVtolDesiredAttitude false updateVtolDesiredAttitude false PathStatusSet &pathStatus break case FLIGHTSTATUS_FLIGHTMODE_POI: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR static void updatePOIBearing PositionActualData positionActual PositionActualGet &positionActual CameraDesiredData cameraDesired CameraDesiredGet &cameraDesired StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired PoiLocationData poi PoiLocationGet &poi float dLoc 3 float yaw=0 float elevation=0 dLoc 0 =positionActual.North-poi.North dLoc 1 =positionActual.East-poi.East dLoc 2 =positionActual.Down-poi.Down if dLoc 1 <0 yaw=RAD2DEG atan2f dLoc 1 dLoc 0 +180 else yaw=RAD2DEG atan2f dLoc 1 dLoc 0 -180 float distance = sqrtf powf dLoc 0 2 +powf dLoc 1 2 if distance!=0 elevation=RAD2DEG atan2f dLoc 2 distance stabDesired.Yaw=yaw stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE cameraDesired.Yaw=yaw cameraDesired.PitchOrServo2=elevation CameraDesiredSet &cameraDesired StabilizationDesiredSet &stabDesired float groundspeed switch pathDesired.Mode case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity break case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break pathStatus.error = progress.error pathStatus.fractional_progress = progress.fractional_progress static void updateVtolDesiredAttitude bool yaw_attitude if yaw_attitude stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE else stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.Yaw = stabSettings.MaximumRate STABILIZATIONSETTINGS_MAXIMUMRATE_YAW * manualControlData.Yaw static void accessoryUpdated UAVObjEvent* ev if ev->obj != AccessoryDesiredHandle return PositionActualData positionActual PositionActualGet &positionActual AccessoryDesiredData accessory PoiLearnSettingsData poiLearn PoiLearnSettingsGet &poiLearn PoiLocationData poi PoiLocationGet &poi if poiLearn.Input != POILEARNSETTINGS_INPUT_NONE if AccessoryDesiredInstGet poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0 &accessory == 0 if accessory.AccessoryVal<-0.5f poi.North=positionActual.North poi.East=positionActual.East poi.Down=positionActual.Down PoiLocationSet &poi
LibrePilot 8206 vtolpathfollower.c Non-Control 589290dbd7 POI orbit + some code run thru code formatter static float poiRadius if vtolpathfollower_enabled xTaskCreate vtolPathFollowerTask signed char * VtolPathFollower STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &pathfollowerTaskHandle TaskMonitorAdd TASKINFO_RUNNING_PATHFOLLOWER pathfollowerTaskHandle return 0 uint8_t optionalModules HWSETTINGS_OPTIONALMODULES_NUMELEM HwSettingsOptionalModulesGet optionalModules if optionalModules HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER == HWSETTINGS_OPTIONALMODULES_ENABLED VtolPathFollowerSettingsInitialize NedAccelInitialize PathDesiredInitialize PathStatusInitialize VelocityDesiredInitialize CameraDesiredInitialize AccessoryDesiredInitialize PoiLearnSettingsInitialize PoiLocationInitialize vtolpathfollower_enabled = true else vtolpathfollower_enabled = false return 0 MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart SystemSettingsData systemSettings FlightStatusData flightStatus portTickType lastUpdateTime VtolPathFollowerSettingsConnectCallback SettingsUpdatedCb AccessoryDesiredConnectCallback accessoryUpdated VtolPathFollowerSettingsGet &vtolpathfollowerSettings lastUpdateTime = xTaskGetTickCount while 1 SystemSettingsGet &systemSettings if systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_WARNING vTaskDelay 1000 continue vTaskDelayUntil &lastUpdateTime vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS updateNedAccel FlightStatusGet &flightStatus PathStatusGet &pathStatus PathDesiredData pathDesired PathDesiredGet &pathDesired switch flightStatus.FlightMode case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_FIXEDATTITUDE: updateFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL break default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break PathStatusSet &pathStatus break case FLIGHTSTATUS_FLIGHTMODE_POI: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break default: northVelIntegral = 0 eastVelIntegral = 0 downVelIntegral = 0 northPosIntegral = 0 eastPosIntegral = 0 downPosIntegral = 0 StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired throttleOffset = stabDesired.Throttle break AlarmsClear SYSTEMALARMS_ALARM_GUIDANCE const float DEADBAND_HIGH = 0.10 const float DEADBAND_LOW = -0.10 float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f PathDesiredData pathDesired PathDesiredGet &pathDesired PositionActualData positionActual PositionActualGet &positionActual CameraDesiredData cameraDesired CameraDesiredGet &cameraDesired StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired PoiLocationData poi PoiLocationGet &poi float dLoc 3 float yaw = 0 float elevation = 0 dLoc 0 = positionActual.North - poi.North dLoc 1 = positionActual.East - poi.East dLoc 2 = positionActual.Down - poi.Down if dLoc 1 < 0 yaw = RAD2DEG atan2f dLoc 1 dLoc 0 + 180 else yaw = RAD2DEG atan2f dLoc 1 dLoc 0 - 180 float distance = sqrtf powf dLoc 0 2 + powf dLoc 1 2 ManualControlCommandData manualControlData ManualControlCommandGet &manualControlData float changeRadius = 0 if manualControlData.Pitch > DEADBAND_HIGH changeRadius = manualControlData.Pitch - DEADBAND_HIGH * dT * 100.0f else if manualControlData.Pitch < DEADBAND_LOW changeRadius = manualControlData.Pitch - DEADBAND_LOW * dT * 100.0f float pathAngle = 0 if manualControlData.Roll > DEADBAND_HIGH pathAngle = - manualControlData.Roll - DEADBAND_HIGH * dT * 300.0f else if manualControlData.Roll < DEADBAND_LOW pathAngle = - manualControlData.Roll - DEADBAND_LOW * dT * 300.0f else if manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH poiRadius = distance + changeRadius if poiRadius >= 3.0f || changeRadius > 0 if pathAngle != 0 || changeRadius != 0 pathDesired.End PATHDESIRED_END_NORTH = poi.North + poiRadius * cosf pathAngle + yaw - 180.0f * DEG2RAD pathDesired.End PATHDESIRED_END_EAST = poi.East + poiRadius * sinf pathAngle + yaw - 180.0f * DEG2RAD pathDesired.StartingVelocity = 1 pathDesired.EndingVelocity = 0 pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT PathDesiredSet &pathDesired if distance >= 3.0f elevation = RAD2DEG atan2f dLoc 2 distance stabDesired.Yaw = yaw + pathAngle / 2.0f stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE CameraDesiredSet &cameraDesired StabilizationDesiredSet &stabDesired float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f float downCommand PathDesiredData pathDesired PathDesiredGet &pathDesired PositionActualData positionActual PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode float groundspeed switch pathDesired.Mode case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity break case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break VelocityDesiredData velocityDesired velocityDesired.North = progress.path_direction 0 * groundspeed velocityDesired.East = progress.path_direction 1 * groundspeed float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP float correction_velocity 2 = progress.correction_direction 0 * error_speed progress.correction_direction 1 * error_speed float total_vel = sqrtf powf correction_velocity 0 2 + powf correction_velocity 1 2 float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel velocityDesired.North += progress.correction_direction 0 * error_speed * scale velocityDesired.East += progress.correction_direction 1 * error_speed * scale float altitudeSetpoint = pathDesired.Start 2 + pathDesired.End 2 - pathDesired.Start 2 * bound progress.fractional_progress 0 1 float downError = altitudeSetpoint - positionActual.Down downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral velocityDesired.Down = bound downCommand -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax pathStatus.error = progress.error pathStatus.fractional_progress = progress.fractional_progress VelocityDesiredSet &velocityDesired float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f PathDesiredData pathDesired PathDesiredGet &pathDesired PositionActualData positionActual VelocityDesiredData velocityDesired PositionActualGet &positionActual VelocityDesiredGet &velocityDesired float northError float eastError float downError float northCommand float eastCommand float downCommand float northPos = 0 eastPos = 0 downPos = 0 switch vtolpathfollowerSettings.PositionSource case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: northPos = positionActual.North eastPos = positionActual.East downPos = positionActual.Down break case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition HomeLocationData homeLocation HomeLocationGet &homeLocation float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD float alt = homeLocation.Altitude float T 3 = alt + 6.378137E6f cosf lat * alt + 6.378137E6f -1.0f float NED 3 = T 0 * gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f * DEG2RAD T 1 * gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f * DEG2RAD T 2 * gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude northPos = NED 0 eastPos = NED 1 downPos = NED 2 break default: PIOS_Assert 0 break northError = pathDesired.End PATHDESIRED_END_NORTH - northPos northPosIntegral = bound northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + northPosIntegral eastError = pathDesired.End PATHDESIRED_END_EAST - eastPos eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + eastPosIntegral float total_vel = sqrtf powf northCommand 2 + powf eastCommand 2 float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel velocityDesired.North = northCommand * scale velocityDesired.East = eastCommand * scale downError = pathDesired.End PATHDESIRED_END_DOWN - downPos downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral velocityDesired.Down = bound downCommand -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax VelocityDesiredSet &velocityDesired StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.Throttle = attitude 3 stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK StabilizationDesiredSet &stabDesired float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f VelocityDesiredData velocityDesired VelocityActualData velocityActual StabilizationDesiredData stabDesired AttitudeActualData attitudeActual NedAccelData nedAccel VtolPathFollowerSettingsData vtolpathfollowerSettings StabilizationSettingsData stabSettings SystemSettingsData systemSettings float northError float northCommand float eastError float eastCommand float downError float downCommand SystemSettingsGet &systemSettings VtolPathFollowerSettingsGet &vtolpathfollowerSettings VelocityActualGet &velocityActual VelocityDesiredGet &velocityDesired StabilizationDesiredGet &stabDesired VelocityDesiredGet &velocityDesired AttitudeActualGet &attitudeActual StabilizationSettingsGet &stabSettings NedAccelGet &nedAccel float northVel = 0 eastVel = 0 downVel = 0 switch vtolpathfollowerSettings.VelocitySource case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: northVel = velocityActual.North eastVel = velocityActual.East downVel = velocityActual.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity northVel = gpsVelocity.North eastVel = gpsVelocity.East downVel = gpsVelocity.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition northVel = gpsPosition.Groundspeed * cosf gpsPosition.Heading * F_PI / 180.0f eastVel = gpsPosition.Groundspeed * sinf gpsPosition.Heading * F_PI / 180.0f downVel = velocityActual.Down break default: PIOS_Assert 0 break ManualControlCommandData manualControlData ManualControlCommandGet &manualControlData northError = velocityDesired.North - northVel northVelIntegral = bound northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward eastError = velocityDesired.East - eastVel eastVelIntegral = bound eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward downError = velocityDesired.Down - downVel downError = -downError downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI -vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD stabDesired.Throttle = bound downCommand + throttleOffset 0 1 stabDesired.Pitch = bound -northCommand * cosf attitudeActual.Yaw * M_PI / 180 + -eastCommand * sinf attitudeActual.Yaw * M_PI / 180 -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.Roll = bound -northCommand * sinf attitudeActual.Yaw * M_PI / 180 + eastCommand * cosf attitudeActual.Yaw * M_PI / 180 -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch if vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE ManualControlCommandData manualControl ManualControlCommandGet &manualControl stabDesired.Throttle = manualControl.Throttle stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE if yaw_attitude stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE else stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.Yaw = stabSettings.MaximumRate STABILIZATIONSETTINGS_MAXIMUMRATE_YAW * manualControlData.Yaw StabilizationDesiredSet &stabDesired float accel 3 float q 4 float Rbe 3 3 float accel_ned 3 AccelsData accels AccelsGet &accels accel 0 = accels.x accel 1 = accels.y accel 2 = accels.z AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual q 0 = attitudeActual.q1 q 1 = attitudeActual.q2 q 2 = attitudeActual.q3 q 3 = attitudeActual.q4 Quaternion2R q Rbe for uint8_t i = 0 i < 3 i++ accel_ned i = 0 for uint8_t j = 0 j < 3 j++ accel_ned i += Rbe j i * accel j accel_ned 2 += 9.81f NedAccelData accelData NedAccelGet &accelData accelData.North = accel_ned 0 accelData.East = accel_ned 1 accelData.Down = accel_ned 2 NedAccelSet &accelData if val < min val = min else if val > max val = max return val VtolPathFollowerSettingsGet &vtolpathfollowerSettings if ev->obj != AccessoryDesiredHandle return AccessoryDesiredData accessory PoiLearnSettingsData poiLearn PoiLearnSettingsGet &poiLearn if poiLearn.Input != POILEARNSETTINGS_INPUT_NONE if AccessoryDesiredInstGet poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0 &accessory == 0 if accessory.AccessoryVal < -0.5f PositionActualData positionActual PositionActualGet &positionActual PoiLocationData poi PoiLocationGet &poi poi.North = positionActual.North poi.East = positionActual.East poi.Down = positionActual.Down PoiLocationSet &poi
LibrePilot 8248 vtolpathfollower.c Non-Control 3512e7a657 Fix allowed frametypes && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX
LibrePilot 9002 vtolpathfollower.c Non-Control 66db978244 Rename Libraries->libraries Modules->modules
LibrePilot 9938 vtolpathfollower.c Non-Control 5f3e0c3e1d Math cleanup #include pathDesired.End PATHDESIRED_END_NORTH = poi.North + poiRadius * cosf DEG2RAD pathAngle + yaw - 180.0f pathDesired.End PATHDESIRED_END_EAST = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f float lat = DEG2RAD homeLocation.Latitude / 10.0e6f T 0 * DEG2RAD gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f T 1 * DEG2RAD gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f T 2 northVel = gpsPosition.Groundspeed * cosf DEG2RAD gpsPosition.Heading eastVel = gpsPosition.Groundspeed * sinf DEG2RAD gpsPosition.Heading stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeActual.Yaw + -eastCommand * sinf DEG2RAD attitudeActual.Yaw stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeActual.Yaw + eastCommand * cosf DEG2RAD attitudeActual.Yaw
LibrePilot 9949 vtolpathfollower.c Non-Control 80c917591e Cherry-pick and hand-merge of Sami s math cleanup commit from branch sambas/diffnext: 5f3e0c3e1da9388ca7572a5cb97d67a002dc0360 #include float lat = DEG2RAD homeLocation.Latitude / 10.0e6f float NED 3 = T 0 * DEG2RAD gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f T 1 * DEG2RAD gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f northVel = gpsPosition.Groundspeed * cosf DEG2RAD gpsPosition.Heading eastVel = gpsPosition.Groundspeed * sinf DEG2RAD gpsPosition.Heading stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeActual.Yaw + -eastCommand * sinf DEG2RAD attitudeActual.Yaw stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeActual.Yaw + eastCommand * cosf DEG2RAD attitudeActual.Yaw
LibrePilot 10052 vtolpathfollower.c Non-Control 84e1a81f8b OP-936: Moves the task monitor code out of the flight library and into PiOS.This move and rework also breaks the dependency of the task monitor on the UAVO subsystem and pushes the responsibility for updating the TaskInfo UAVO into the System module.+review OPReview #include #include taskinfo.h #include paths.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_PATHFOLLOWER pathfollowerTaskHandle
LibrePilot 10135 vtolpathfollower.c Non-Control a2d8544931 OP-931: adds -Wextra compiler option for the flight code and makes the bazillion code changes requiredto make the flight code compile again. Needs careful review particularly all the fixes for thesigned vs unsigned comparisons.+review OPReview-459 static void vtolPathFollowerTask __attribute__ unused void *parameters static void SettingsUpdatedCb __attribute__ unused UAVObjEvent *ev
LibrePilot 10296 vtolpathfollower.c Non-Control ed68fbe68d OP-951: Adds -Wshadow to flight CFLAGS and fixes resulting compilation breakage.+review OPReview
LibrePilot 11017 vtolpathfollower.c Non-Control 80caceb984 make uncrustify_all;make uncrustify_all; * @brief This module compared @ref PositionActual to @ref PathDesired * This module will periodically update the value of the @ref StabilizationDesired object based on #include pathdesired.h #define MAX_QUEUE_SIZE 4 #define TASK_PRIORITY tskIDLE_PRIORITY + 2 static void SettingsUpdatedCb UAVObjEvent *ev static void updateFixedAttitude float *attitude static void accessoryUpdated UAVObjEvent *ev MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart static float eastVelIntegral = 0 static float downVelIntegral = 0 static float eastPosIntegral = 0 static float downPosIntegral = 0 static float throttleOffset = 0 && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_FIXEDATTITUDE: updateFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR PathStatusSet &pathStatus break case FLIGHTSTATUS_FLIGHTMODE_POI: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break default: northVelIntegral = 0 eastVelIntegral = 0 downVelIntegral = 0 northPosIntegral = 0 eastPosIntegral = 0 downPosIntegral = 0 StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired throttleOffset = stabDesired.Throttle break const float DEADBAND_LOW = -0.10f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 + 180.0f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 - 180.0f pathDesired.End PATHDESIRED_END_EAST = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f pathDesired.EndingVelocity = 0.0f * Takes in @ref PositionActual and compares it to @ref PathDesired case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity break case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break velocityDesired.East = progress.path_direction 1 * groundspeed float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East += progress.correction_direction 1 * error_speed * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral pathStatus.error = progress.error * Takes in @ref PositionActual and compares it to @ref PositionDesired case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: northPos = positionActual.North eastPos = positionActual.East downPos = positionActual.Down break case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition HomeLocationData homeLocation HomeLocationGet &homeLocation float lat = DEG2RAD homeLocation.Latitude / 10.0e6f float alt = homeLocation.Altitude float T 3 = alt + 6.378137E6f cosf lat * alt + 6.378137E6f -1.0f float NED 3 = T 0 * DEG2RAD gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f T 1 * DEG2RAD gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f T 2 * gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude northPos = NED 0 eastPos = NED 1 downPos = NED 2 break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + northPosIntegral eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East = eastCommand * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral static void updateFixedAttitude float *attitude stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: northVel = velocityActual.North eastVel = velocityActual.East downVel = velocityActual.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity northVel = gpsVelocity.North eastVel = gpsVelocity.East downVel = gpsVelocity.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition northVel = gpsPosition.Groundspeed * cosf DEG2RAD gpsPosition.Heading eastVel = gpsPosition.Groundspeed * sinf DEG2RAD gpsPosition.Heading downVel = velocityActual.Down break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeActual.Yaw + -eastCommand * sinf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeActual.Yaw + eastCommand * cosf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE for uint8_t j = 0 j < 3 j++ accelData.East = accel_ned 1 accelData.Down = accel_ned 2 static void accessoryUpdated UAVObjEvent *ev if ev->obj != AccessoryDesiredHandle poi.East = positionActual.East poi.Down = positionActual.Down
LibrePilot 11225 vtolpathfollower.c Non-Control 5284195c29 Refaktored sensor and state UAVObjects consistently into XXYSensor and XXYState * @brief This module compared @ref PositionState to @ref PathDesired * Input object: PositionState * @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported #include accelstate.h #include attitudestate.h #include positionstate.h #include velocitystate.h PositionStateData positionState PositionStateGet &positionState dLoc 0 = positionState.North - poi.North dLoc 1 = positionState.East - poi.East dLoc 2 = positionState.Down - poi.Down * Takes in @ref PositionState and compares it to @ref PathDesired PositionStateData positionState PositionStateGet &positionState positionState.North positionState.East positionState.Down float downError = altitudeSetpoint - positionState.Down * Takes in @ref PositionState and compares it to @ref PositionDesired PositionStateData positionState PositionStateGet &positionState northPos = positionState.North eastPos = positionState.East downPos = positionState.Down * Takes in @ref NedState which has the acceleration in the * @ref VelocityState against the @ref VelocityDesired VelocityStateData velocityState AttitudeStateData attitudeState VelocityStateGet &velocityState AttitudeStateGet &attitudeState northVel = velocityState.North eastVel = velocityState.East downVel = velocityState.Down downVel = velocityState.Down stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeState.Yaw + -eastCommand * sinf DEG2RAD attitudeState.Yaw stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeState.Yaw + eastCommand * cosf DEG2RAD attitudeState.Yaw AccelStateData accelState AccelStateGet &accelState accel 0 = accelState.x accel 1 = accelState.y accel 2 = accelState.z AttitudeStateData attitudeState AttitudeStateGet &attitudeState q 0 = attitudeState.q1 q 1 = attitudeState.q2 q 2 = attitudeState.q3 q 3 = attitudeState.q4 PositionStateData positionState PositionStateGet &positionState poi.North = positionState.North poi.East = positionState.East poi.Down = positionState.Down
LibrePilot 11602 vtolpathfollower.c Non-Control daf329d8f5 uncrustification (again) * @brief This module compared @ref PositionActual to @ref PathDesired * This module will periodically update the value of the @ref StabilizationDesired object based on #include pathdesired.h #define MAX_QUEUE_SIZE 4 #define TASK_PRIORITY tskIDLE_PRIORITY + 2 static void SettingsUpdatedCb UAVObjEvent *ev static void updateFixedAttitude float *attitude static void accessoryUpdated UAVObjEvent *ev MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart static float eastVelIntegral = 0 static float downVelIntegral = 0 static float eastPosIntegral = 0 static float downPosIntegral = 0 static float throttleOffset = 0 && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_FIXEDATTITUDE: updateFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR PathStatusSet &pathStatus break case FLIGHTSTATUS_FLIGHTMODE_POI: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break default: northVelIntegral = 0 eastVelIntegral = 0 downVelIntegral = 0 northPosIntegral = 0 eastPosIntegral = 0 downPosIntegral = 0 StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired throttleOffset = stabDesired.Throttle break const float DEADBAND_LOW = -0.10f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 + 180.0f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 - 180.0f pathDesired.End PATHDESIRED_END_EAST = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f pathDesired.EndingVelocity = 0.0f * Takes in @ref PositionActual and compares it to @ref PathDesired case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity break case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break velocityDesired.East = progress.path_direction 1 * groundspeed float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East += progress.correction_direction 1 * error_speed * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral pathStatus.error = progress.error * Takes in @ref PositionActual and compares it to @ref PositionDesired case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: northPos = positionActual.North eastPos = positionActual.East downPos = positionActual.Down break case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition HomeLocationData homeLocation HomeLocationGet &homeLocation float lat = DEG2RAD homeLocation.Latitude / 10.0e6f float alt = homeLocation.Altitude float T 3 = alt + 6.378137E6f cosf lat * alt + 6.378137E6f -1.0f float NED 3 = T 0 * DEG2RAD gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f T 1 * DEG2RAD gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f T 2 * gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude northPos = NED 0 eastPos = NED 1 downPos = NED 2 break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + northPosIntegral eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East = eastCommand * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral static void updateFixedAttitude float *attitude stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: northVel = velocityActual.North eastVel = velocityActual.East downVel = velocityActual.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity northVel = gpsVelocity.North eastVel = gpsVelocity.East downVel = gpsVelocity.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition northVel = gpsPosition.Groundspeed * cosf DEG2RAD gpsPosition.Heading eastVel = gpsPosition.Groundspeed * sinf DEG2RAD gpsPosition.Heading downVel = velocityActual.Down break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeActual.Yaw + -eastCommand * sinf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeActual.Yaw + eastCommand * cosf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE for uint8_t j = 0 j < 3 j++ accelData.East = accel_ned 1 accelData.Down = accel_ned 2 static void accessoryUpdated UAVObjEvent *ev if ev->obj != AccessoryDesiredHandle poi.East = positionActual.East poi.Down = positionActual.Down
LibrePilot 11861 vtolpathfollower.c Non-Control 900f643bbd Reformat source code with make uncrustify_all run twice. NO CODE CHANGES * @brief This module compared @ref PositionActual to @ref PathDesired * This module will periodically update the value of the @ref StabilizationDesired object based on #include pathdesired.h #define MAX_QUEUE_SIZE 4 #define TASK_PRIORITY tskIDLE_PRIORITY + 2 static void SettingsUpdatedCb UAVObjEvent *ev static void updateFixedAttitude float *attitude static void accessoryUpdated UAVObjEvent *ev MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart static float eastVelIntegral = 0 static float downVelIntegral = 0 static float eastPosIntegral = 0 static float downPosIntegral = 0 static float throttleOffset = 0 && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_FIXEDATTITUDE: updateFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR PathStatusSet &pathStatus break case FLIGHTSTATUS_FLIGHTMODE_POI: if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break default: northVelIntegral = 0 eastVelIntegral = 0 downVelIntegral = 0 northPosIntegral = 0 eastPosIntegral = 0 downPosIntegral = 0 StabilizationDesiredData stabDesired StabilizationDesiredGet &stabDesired throttleOffset = stabDesired.Throttle break const float DEADBAND_LOW = -0.10f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 + 180.0f yaw = RAD2DEG atan2f dLoc 1 dLoc 0 - 180.0f pathDesired.End PATHDESIRED_END_EAST = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f pathDesired.EndingVelocity = 0.0f * Takes in @ref PositionActual and compares it to @ref PathDesired case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity break case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + pathDesired.EndingVelocity - pathDesired.StartingVelocity * bound progress.fractional_progress 0 1 if progress.fractional_progress > 1 groundspeed = 0 break velocityDesired.East = progress.path_direction 1 * groundspeed float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East += progress.correction_direction 1 * error_speed * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral pathStatus.error = progress.error * Takes in @ref PositionActual and compares it to @ref PositionDesired case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: northPos = positionActual.North eastPos = positionActual.East downPos = positionActual.Down break case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition HomeLocationData homeLocation HomeLocationGet &homeLocation float lat = DEG2RAD homeLocation.Latitude / 10.0e6f float alt = homeLocation.Altitude float T 3 = alt + 6.378137E6f cosf lat * alt + 6.378137E6f -1.0f float NED 3 = T 0 * DEG2RAD gpsPosition.Latitude - homeLocation.Latitude / 10.0e6f T 1 * DEG2RAD gpsPosition.Longitude - homeLocation.Longitude / 10.0e6f T 2 * gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude northPos = NED 0 eastPos = NED 1 downPos = NED 2 break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP + northPosIntegral eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI -vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT vtolpathfollowerSettings.HorizontalPosPI VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT float scale = 1 if total_vel > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.East = eastCommand * scale -vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalPosPI VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP + downPosIntegral static void updateFixedAttitude float *attitude stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_YAW = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: northVel = velocityActual.North eastVel = velocityActual.East downVel = velocityActual.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity northVel = gpsVelocity.North eastVel = gpsVelocity.East downVel = gpsVelocity.Down break case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: GPSPositionData gpsPosition GPSPositionGet &gpsPosition northVel = gpsPosition.Groundspeed * cosf DEG2RAD gpsPosition.Heading eastVel = gpsPosition.Groundspeed * sinf DEG2RAD gpsPosition.Heading downVel = velocityActual.Down break default: PIOS_Assert 0 break -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward -vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT downCommand = downError * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD stabDesired.Pitch = bound -northCommand * cosf DEG2RAD attitudeActual.Yaw + -eastCommand * sinf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.Roll = bound -northCommand * sinf DEG2RAD attitudeActual.Yaw + eastCommand * cosf DEG2RAD attitudeActual.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.StabilizationMode STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE for uint8_t j = 0 j < 3 j++ accelData.East = accel_ned 1 accelData.Down = accel_ned 2 static void accessoryUpdated UAVObjEvent *ev if ev->obj != AccessoryDesiredHandle poi.East = positionActual.East poi.Down = positionActual.Down
LibrePilot 12114 vtolpathfollower.c Non-Control 9b95af2006 refaktored GPS Sensor UAVObjects #include gpsvelocitysensor.h #include gpspositionsensor.h GPSPositionSensorData gpsPosition GPSPositionSensorGet &gpsPosition GPSVelocitySensorData gpsVelocity GPSVelocitySensorGet &gpsVelocity GPSPositionSensorData gpsPosition GPSPositionSensorGet &gpsPosition
LibrePilot 12194 vtolpathfollower.c Non-Control 23b2907d08 OP-976: Add the ; backThis compile successfuly with make all_flight MODULE_INITCALL VtolPathFollowerInitialize VtolPathFollowerStart
LibrePilot 12459 vtolpathfollower.c Non-Control 9e1acc3165 OP-1058: fix needed for fw_revolution code compilation pathDesired.End.fields.North = poi.North + poiRadius * cosf DEG2RAD pathAngle + yaw - 180.0f pathDesired.End.fields.East = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE path_progress pathDesired.Start.data pathDesired.End.data cur &progress pathDesired.Mode float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp float altitudeSetpoint = pathDesired.Start.fields.Down + pathDesired.End.fields.Down - pathDesired.Start.fields.Down * bound progress.fractional_progress 0 1 downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit vtolpathfollowerSettings.VerticalPosPI.fields.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral northError = pathDesired.End.fields.North - northPos northPosIntegral = bound northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + northPosIntegral eastError = pathDesired.End.fields.East - eastPos eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit eastCommand = eastError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + eastPosIntegral downError = pathDesired.End.fields.Down - downPos downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit vtolpathfollowerSettings.VerticalPosPI.fields.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK northVelIntegral = bound northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd eastVelIntegral = bound eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.fields.Ki -vtolpathfollowerSettings.VerticalVelPID.fields.ILimit vtolpathfollowerSettings.VerticalVelPID.fields.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalVelPID.fields.Kp + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.fields.Kd stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.Yaw = stabSettings.MaximumRate.fields.Yaw * manualControlData.Yaw
LibrePilot 12472 vtolpathfollower.c Non-Control 01d963affd OP-1058 uncrustify stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE
LibrePilot 12499 vtolpathfollower.c Control e91bc28667 OP-1058 Get rid of Unions.They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions.It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays+review OPReview-552 #include pathDesired.End.North = poi.North + poiRadius * cosf DEG2RAD pathAngle + yaw - 180.0f pathDesired.End.East = poi.East + poiRadius * sinf DEG2RAD pathAngle + yaw - 180.0f stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE path_progress cast_struct_to_array pathDesired.Start pathDesired.Start.North cast_struct_to_array pathDesired.End pathDesired.End.North cur &progress pathDesired.Mode float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp float altitudeSetpoint = pathDesired.Start.Down + pathDesired.End.Down - pathDesired.Start.Down * bound progress.fractional_progress 0 1 downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral northError = pathDesired.End.North - northPos northPosIntegral = bound northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit northCommand = northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral eastError = pathDesired.End.East - eastPos eastPosIntegral = bound eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit eastCommand = eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral downError = pathDesired.End.Down - downPos downPosIntegral = bound downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK northVelIntegral = bound northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki -vtolpathfollowerSettings.HorizontalVelPID.ILimit vtolpathfollowerSettings.HorizontalVelPID.ILimit northCommand = northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd eastVelIntegral = bound eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki -vtolpathfollowerSettings.HorizontalVelPID.ILimit vtolpathfollowerSettings.HorizontalVelPID.ILimit eastCommand = eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki -vtolpathfollowerSettings.VerticalVelPID.ILimit vtolpathfollowerSettings.VerticalVelPID.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw
LibrePilot 12513 vtolpathfollower.c Control 0db9a9bf8d OP-1058 Add xxxGet and xxxSet functions to handle multielement fields as structfor example:EKFStateVariancePSet(EKFStateVariancePData *NewP);EKFStateVariancePGet(EKFStateVariancePData *NewP);Also in this case array accessors are renamed as xxxArrayGet/Set:EKFStateVariancePArraySet(float *NewP);EKFStateVariancePArrayGet(float *NewP);Nothing changes for anonymous items as default functions continues to deal with arrays+review OPReview-552 HwSettingsOptionalModulesData optionalModules HwSettingsOptionalModulesGet &optionalModules if optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED
LibrePilot 12600 vtolpathfollower.c Control d625242a45 OP-984 Updated vtolpathfollower to handle PID banks #include stabilizationbank.h StabilizationBankData stabSettings StabilizationBankGet &stabSettings
LibrePilot 12797 vtolpathfollower.c Non-Control b5a27d05e7 OP-1197 configurable failsafes OP-1219 thrust control OP-1217 separate manualcontrolsettings apart static float thrustOffset = 0 thrustOffset = stabDesired.Thrust stabDesired.Roll = attitude 0 stabDesired.Pitch = attitude 1 stabDesired.Yaw = attitude 2 stabDesired.Thrust = attitude 3 downVelIntegral = bound downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki -vtolpathfollowerSettings.VerticalVelPID.ILimit vtolpathfollowerSettings.VerticalVelPID.ILimit downCommand = downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd stabDesired.Thrust = bound downCommand + thrustOffset 0 1 if vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE stabDesired.Thrust = manualControl.Thrust
LibrePilot 12817 vtolpathfollower.c Non-Control 7dd911efdb OP-1217 change the way modules are enabled/disabled based on flightstatus if flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE if flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE if flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude true updatePOIBearing else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR else if pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT updateEndpointVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK else AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR pathStatus.UID = pathDesired.UID pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS switch pathDesired.Mode case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity updateVtolDesiredAttitude false AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_FIXEDATTITUDE: updateFixedAttitude pathDesired.ModeParameters AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_OK break case PATHDESIRED_MODE_DISARMALARM: AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL break default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_ERROR break PathStatusSet &pathStatus else
LibrePilot 12840 vtolpathfollower.c Non-Control cdca8d835c OP-1216 manualcontrol refactoring #include manualcontrolcommand.h
LibrePilot 12932 vtolpathfollower.c Control 654d842c22 OP-1288 Remove PositionSource from VtolPathFollowerSettings northError = pathDesired.End.North - positionState.North eastError = pathDesired.End.East - positionState.East downError = pathDesired.End.Down - positionState.Down case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED: case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
LibrePilot 12997 vtolpathfollower.c Control e9d1a2af4b OP-1309 get rid of this horrible bound() function code duplication throughout entire flight code and put it into libraries/math groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf progress.fractional_progress 0 1 + pathDesired.EndingVelocity - pathDesired.StartingVelocity * boundf progress.fractional_progress 0 1 float altitudeSetpoint = pathDesired.Start.Down + pathDesired.End.Down - pathDesired.Start.Down * boundf progress.fractional_progress 0 1 downPosIntegral = boundf downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit velocityDesired.Down = boundf downCommand -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax northPosIntegral = boundf northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit eastPosIntegral = boundf eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit downPosIntegral = boundf downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit velocityDesired.Down = boundf downCommand -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax northVelIntegral = boundf northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki -vtolpathfollowerSettings.HorizontalVelPID.ILimit vtolpathfollowerSettings.HorizontalVelPID.ILimit eastVelIntegral = boundf eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki -vtolpathfollowerSettings.HorizontalVelPID.ILimit vtolpathfollowerSettings.HorizontalVelPID.ILimit downVelIntegral = boundf downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki -vtolpathfollowerSettings.VerticalVelPID.ILimit vtolpathfollowerSettings.VerticalVelPID.ILimit stabDesired.Thrust = boundf downCommand + thrustOffset 0 1 stabDesired.Pitch = boundf -northCommand * cosf DEG2RAD attitudeState.Yaw + -eastCommand * sinf DEG2RAD attitudeState.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch stabDesired.Roll = boundf -northCommand * sinf DEG2RAD attitudeState.Yaw + eastCommand * cosf DEG2RAD attitudeState.Yaw -vtolpathfollowerSettings.MaxRollPitch vtolpathfollowerSettings.MaxRollPitch
LibrePilot 13056 vtolpathfollower.c Non-Control 6d3ade4947 OP-1308 Set the same logic for alarms AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL AlarmsSet SYSTEMALARMS_ALARM_GUIDANCE SYSTEMALARMS_ALARM_CRITICAL
LibrePilot 13083 vtolpathfollower.c Non-Control 0ea1ccf5c3 OP-1316 fix gravity constants HomeLocationInitialize static float gravity HomeLocationg_eGet &gravity accel_ned 2 += gravity
LibrePilot 13093 vtolpathfollower.c Control 69b5f41e58 OP-1309 correctly set StabilizationMode.Thrust in all other modules that set StabilizationDesired (Autotune Pathfollowers) stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
LibrePilot 13178 vtolpathfollower.c Control 36e304f579 OP-1274 fixes for revolution xTaskCreate vtolPathFollowerTask const char * VtolPathFollower STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &pathfollowerTaskHandle
LibrePilot 13355 vtolpathfollower.c Non-Control eb5deb3eca OP-1274 Remove unneeded cast from task name in xTaskCreate call xTaskCreate vtolPathFollowerTask VtolPathFollower STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &pathfollowerTaskHandle
LibrePilot 13371 vtolpathfollower.c Control 30491c7992 OP-1374 - Autostart the relevant PathFollower for known frame types #include FrameType_t frameType = GetCurrentFrameType if optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED || frameType == FRAME_TYPE_MULTIROTOR
LibrePilot 13380 vtolpathfollower.c Control a46480a99b OP-1063 Multi_config_Hexa_fixes : Added real HexaX renamed HexaX > HexaHAdded artwork in config and wizard svg. Cleanup on svg connection diagram (layers)DO NOT USE until mixers are fixed && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH
LibrePilot 13382 vtolpathfollower.c Control c26e33964a OP-1063 Multi_config_Hexa_fixes Added OctoX define new mixer table for Octo(p) and OctoX. New motor definition for OctoX (NNE ENE...) Fixes for svg frames. Defined default settings for OctoP/X in setupwizard (but disabled) && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX
LibrePilot 13383 vtolpathfollower.c Control 1b447ee644 OP-1063 Multi_config_Hexa_fixes Stupid typo error with OctoX in VtolPathFollower. && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX
LibrePilot 13384 vtolpathfollower.c Non-Control 62755c4cab unrustify && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI && systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH
LibrePilot 13396 vtolpathfollower.c Control 3e2961420d Add a 3D mode to path_endpoint. This allows FlyDirect waypoints to dovertical movements without changing position. float speed speed = pathDesired.EndingVelocity speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf progress.fractional_progress 0 1 speed = 0 speed = pathDesired.StartingVelocity speed = 0 velocityDesired.North = progress.path_direction 0 * speed velocityDesired.East = progress.path_direction 1 * speed velocityDesired.Down = progress.path_direction 2 * speed downCommand = velocityDesired.Down + downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral
LibrePilot 13399 vtolpathfollower.c Control 065ba1a0c8 Adds a 3D mode to path_vector. This allows vertical path segments withoutposition changes. PathStatus now also contains correction_direction andpath_direction to make path following behaviour more transparent. float current_position 3 = positionState.North positionState.East positionState.Down current_position &progress pathDesired.Mode northPosIntegral += progress.correction_direction 0 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT eastPosIntegral += progress.correction_direction 1 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT downPosIntegral += progress.correction_direction 2 * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT northPosIntegral = boundf northPosIntegral -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit eastPosIntegral = boundf eastPosIntegral -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit downPosIntegral = boundf downPosIntegral -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit velocityDesired.North = progress.path_direction 0 * speed + northPosIntegral + progress.correction_direction 0 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp velocityDesired.East = progress.path_direction 1 * speed + eastPosIntegral + progress.correction_direction 1 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp velocityDesired.Down = progress.path_direction 2 * speed + downPosIntegral + progress.correction_direction 2 * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp float groundspeedDesired = sqrtf powf velocityDesired.North 2 + powf velocityDesired.East 2 if groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired velocityDesired.Down = boundf velocityDesired.Down -vtolpathfollowerSettings.VerticalVelMax vtolpathfollowerSettings.VerticalVelMax pathStatus.error = progress.error pathStatus.fractional_progress = progress.fractional_progress pathStatus.path_direction_north = progress.path_direction 0 pathStatus.path_direction_east = progress.path_direction 1 pathStatus.path_direction_down = progress.path_direction 2 pathStatus.correction_direction_north = progress.correction_direction 0 pathStatus.correction_direction_east = progress.correction_direction 1 pathStatus.correction_direction_down = progress.correction_direction 2
LibrePilot 13442 vtolpathfollower.c Non-Control 8944419b0f OP-1156 refactored pathfollower - new unified module as delayed callback - compiles but untested + pathDesired.EndingVelocity - pathDesired.StartingVelocity * boundf progress.fractional_progress 0 1 northPosIntegral = boundf northPosIntegral -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit eastPosIntegral = boundf eastPosIntegral -vtolpathfollowerSettings.HorizontalPosPI.ILimit vtolpathfollowerSettings.HorizontalPosPI.ILimit downPosIntegral = boundf downPosIntegral -vtolpathfollowerSettings.VerticalPosPI.ILimit vtolpathfollowerSettings.VerticalPosPI.ILimit progress.correction_direction 0 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp progress.correction_direction 1 * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp progress.correction_direction 2 * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp pathStatus.error = progress.error
LibrePilot 13474 vtolpathfollower.c Non-Control 4653c77296 OP-1156 removed old pathfollower implementations
LibrePilot 5380 pathplanner.c Control 3eac69e953 Create stub for waypoint navigation #include openpilot.h #include flightstatus.h #include positionactual.h #include positiondesired.h #include waypoint.h #include waypointactive.h #define STACK_SIZE_BYTES 1500 #define TASK_PRIORITY tskIDLE_PRIORITY+1 #define MAX_QUEUE_SIZE 2 static xTaskHandle taskHandle static xQueueHandle queue static void pathPlannerTask void *parameters int32_t PathPlannerStart taskHandle = NULL xTaskCreate pathPlannerTask signed char * PathPlanner STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &taskHandle TaskMonitorAdd TASKINFO_RUNNING_PATHPLANNER taskHandle return 0 int32_t PathPlannerInitialize taskHandle = NULL WaypointInitialize WaypointActiveInitialize queue = xQueueCreate MAX_QUEUE_SIZE sizeof UAVObjEvent return 0 MODULE_INITCALL PathPlannerInitialize PathPlannerStart static void pathPlannerTask void *parameters FlightStatusData flightStatus PositionActualData positionActual WaypointActiveData waypointActive WaypointData waypoint const float MIN_RADIUS = 2.0f while 1 FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER continue PositionActualGet &positionActual WaypointActiveGet &waypointActive WaypointInstGet waypointActive.Index &waypoint float r2 = powf positionActual.North - waypoint.Position WAYPOINT_POSITION_NORTH 2 + powf positionActual.East - waypoint.Position WAYPOINT_POSITION_EAST 2 + powf positionActual.Down - waypoint.Position WAYPOINT_POSITION_DOWN 2 if r2 < MIN_RADIUS * MIN_RADIUS if waypoint.Action == WAYPOINT_ACTION_NEXT waypointActive.Index++ WaypointActiveSet &waypointActive if WaypointInstGet waypointActive.Index &waypoint != 0 continue PositionDesiredData positionDesired PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired vTaskDelay 10
LibrePilot 5381 pathplanner.c Control d4488512c0 Add RTH waypoint action else if waypointAction == WAYPOINT_ACTION_RTH PositionDesiredData positionDesired PositionDesiredGet &positionDesired positionDesired.North = 0 positionDesired.East = 0 positionDesired.Down = -20 PositionDesiredSet &positionDesired
LibrePilot 5385 pathplanner.c Control ba1ade33cb Get navigation working and program hardcoded flight path #define STACK_SIZE_BYTES 2500 int32_t bad_inits int32_t bad_reads PositionDesiredData positionDesired for uint32_t i = 0 i < 20 i++ waypoint.Position 1 = 30 * cos i / 10.0 * M_PI waypoint.Position 0 = 50 * sin i / 10.0 * M_PI waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance bad_inits += WaypointInstSet i &waypoint != 0 for uint32_t i = 20 i < 35 i++ waypoint.Position 1 = 55 + 20 * cos i / 10.0 * M_PI - M_PI / 2 waypoint.Position 0 = 25 + 25 * sin i / 10.0 * M_PI - M_PI / 2 waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance bad_inits += WaypointInstSet i &waypoint != 0 waypoint.Position 1 = 35 waypoint.Position 0 = -50 waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_RTH WaypointCreateInstance WaypointInstSet 35 &waypoint vTaskDelay 100 bad_reads += WaypointInstGet waypointActive.Index &waypoint != 0 switch waypoint.Action case WAYPOINT_ACTION_NEXT: waypointActive.Index++ WaypointActiveSet &waypointActive if WaypointInstGet waypointActive.Index &waypoint != 0 continue break case WAYPOINT_ACTION_RTH: PositionDesiredGet &positionDesired positionDesired.North = 0 positionDesired.East = 0 positionDesired.Down = -20 PositionDesiredSet &positionDesired break default: PIOS_DEBUG_Assert 0 PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired
LibrePilot 5386 pathplanner.c Control 32f76db8ae Only update position desired when you change waypoints active bool pathplanner_active = false if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER pathplanner_active = false if pathplanner_active == false WaypointActiveSet &waypointActive waypointActive.Index = 0 WaypointActiveSet &waypointActive WaypointInstGet waypointActive.Index &waypoint PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired pathplanner_active = true continue WaypointInstGet waypointActive.Index &waypoint PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired
LibrePilot 5389 pathplanner.c Control c26cceb47b If the waypoint index is changed then update teh position desired. This shouldprobably be done with change callbacks because currently changing the waypointsthemselves doesn t update position desired. static void updatePositionDesired uint32_t last_index=0 updatePositionDesired last_index = waypointActive.Index if last_index != waypointActive.Index updatePositionDesired updatePositionDesired last_index = waypointActive.Index static void updatePositionDesired PositionDesiredData positionDesired WaypointActiveData waypointActive WaypointData waypoint WaypointInstGet waypointActive.Index &waypoint PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired
LibrePilot 5390 pathplanner.c Control bfb0c6935e Make pathplanner use callbacks on the waypoints and their index to pick upchanges from the GCS while flying static WaypointActiveData waypointActive static WaypointData waypoint static void waypointsUpdated UAVObjEvent * ev WaypointConnectCallback waypointsUpdated WaypointActiveConnectCallback waypointsUpdated vTaskDelay 20 FlightStatusGet &flightStatus WaypointActiveGet &waypointActive static void waypointsUpdated UAVObjEvent * ev FlightStatusData flightStatus FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER return WaypointActiveGet &waypointActive WaypointInstGet waypointActive.Index &waypoint PositionDesiredData positionDesired
LibrePilot 5391 pathplanner.c Control 5827e3f565 Add box around logo path static void createPath createPath const float MIN_RADIUS = 4.0f static void createPath WaypointData waypoint for uint32_t i = 0 i < 20 i++ waypoint.Position 1 = 30 * cos i / 19.0 * 2 * M_PI waypoint.Position 0 = 50 * sin i / 19.0 * 2 * M_PI waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance bad_inits += WaypointInstSet i &waypoint != 0 for uint32_t i = 20 i < 35 i++ waypoint.Position 1 = 55 + 20 * cos i / 10.0 * M_PI - M_PI / 2 waypoint.Position 0 = 25 + 25 * sin i / 10.0 * M_PI - M_PI / 2 waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance bad_inits += WaypointInstSet i &waypoint != 0 waypoint.Position 1 = 35 waypoint.Position 0 = -50 waypoint.Position 2 = -50 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 35 &waypoint waypoint.Position 1 = 35 waypoint.Position 0 = -60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 36 &waypoint waypoint.Position 1 = 85 waypoint.Position 0 = -60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 37 &waypoint waypoint.Position 1 = 85 waypoint.Position 0 = 60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 38 &waypoint waypoint.Position 1 = -40 waypoint.Position 0 = 60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 39 &waypoint waypoint.Position 1 = -40 waypoint.Position 0 = -60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 40 &waypoint waypoint.Position 1 = 35 waypoint.Position 0 = -60 waypoint.Position 2 = -30 waypoint.Action = WAYPOINT_ACTION_NEXT WaypointCreateInstance WaypointInstSet 41 &waypoint * @ * @ */
LibrePilot 5528 pathplanner.c Control aa6c8a451d Shrink some stack sizes #define STACK_SIZE_BYTES 512
LibrePilot 5551 pathplanner.c Control a42d756b3e Add path navigation mode to the path planner module. I don t like how this isdone because the mode is in the settings so can t be command from path planner.I.e. it would be nice to be able to say do these paths then go to position holdmode. #include paths.h #include guidancesettings.h #include pathdesired.h static GuidanceSettingsData guidanceSettings PathDesiredData pathDesired GuidanceSettingsGet &guidanceSettings switch guidanceSettings.PathMode case GUIDANCESETTINGS_PATHMODE_ENDPOINT: PositionActualGet &positionActual float r2 = powf positionActual.North - waypoint.Position WAYPOINT_POSITION_NORTH 2 + powf positionActual.East - waypoint.Position WAYPOINT_POSITION_EAST 2 + powf positionActual.Down - waypoint.Position WAYPOINT_POSITION_DOWN 2 if r2 < MIN_RADIUS * MIN_RADIUS switch waypoint.Action case WAYPOINT_ACTION_NEXT: waypointActive.Index++ WaypointActiveSet &waypointActive break case WAYPOINT_ACTION_RTH: PositionDesiredGet &positionDesired positionDesired.North = 0 positionDesired.East = 0 positionDesired.Down = -20 PositionDesiredSet &positionDesired break default: PIOS_DEBUG_Assert 0 break case GUIDANCESETTINGS_PATHMODE_PATH: PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress if progress.fractional_progress >= 1 switch waypoint.Action case WAYPOINT_ACTION_NEXT: waypointActive.Index++ WaypointActiveSet &waypointActive break case WAYPOINT_ACTION_RTH: PositionDesiredGet &positionDesired positionDesired.North = 0 positionDesired.East = 0 positionDesired.Down = -20 PositionDesiredSet &positionDesired break default: PIOS_DEBUG_Assert 0 break GuidanceSettingsGet &guidanceSettings switch guidanceSettings.PathMode case GUIDANCESETTINGS_PATHMODE_ENDPOINT: PositionDesiredData positionDesired PositionDesiredGet &positionDesired positionDesired.North = waypoint.Position WAYPOINT_POSITION_NORTH positionDesired.East = waypoint.Position WAYPOINT_POSITION_EAST positionDesired.Down = waypoint.Position WAYPOINT_POSITION_DOWN PositionDesiredSet &positionDesired break case GUIDANCESETTINGS_PATHMODE_PATH: PathDesiredData pathDesired pathDesired.StartingVelocity = 2 pathDesired.EndingVelocity = 2 pathDesired.End PATHDESIRED_END_NORTH = waypoint.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypoint.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypoint.Position WAYPOINT_POSITION_DOWN if waypointActive.Index == 0 PositionActualData positionActual PositionActualGet &positionActual pathDesired.Start PATHDESIRED_START_NORTH = positionActual.North pathDesired.Start PATHDESIRED_START_EAST = positionActual.East pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start PATHDESIRED_END_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_END_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_END_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN PathDesiredSet &pathDesired break
LibrePilot 5552 pathplanner.c Control a1351fcdbe Make the path velocity come from the waypoint. pathDesired.EndingVelocity = sqrtf powf waypoint.Velocity WAYPOINT_VELOCITY_NORTH 2 + powf waypoint.Velocity WAYPOINT_VELOCITY_EAST 2 pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down - 1 pathDesired.StartingVelocity = pathDesired.EndingVelocity pathDesired.StartingVelocity = sqrtf powf waypointPrev.Velocity WAYPOINT_VELOCITY_NORTH 2 + powf waypointPrev.Velocity WAYPOINT_VELOCITY_EAST 2 waypoint.Velocity 0 = 2
LibrePilot 5579 pathplanner.c Control 6a094ac326 Increase stack for the path planner #define STACK_SIZE_BYTES 1024
LibrePilot 5761 pathplanner.c Non-Control 2857b58f8e Migrate path planner from position desired to path desired PathDesiredGet &pathDesired pathDesired.End PATHDESIRED_END_NORTH = 0 pathDesired.End PATHDESIRED_END_EAST = 0 pathDesired.End PATHDESIRED_END_DOWN = -20 pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT PathDesiredSet &pathDesired PathDesiredGet &pathDesired pathDesired.End PATHDESIRED_END_NORTH = 0 pathDesired.End PATHDESIRED_END_EAST = 0 pathDesired.End PATHDESIRED_END_DOWN = -20 pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT PathDesiredSet &pathDesired PathDesiredData pathDesired PathDesiredGet &pathDesired pathDesired.End PATHDESIRED_END_NORTH = waypoint.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypoint.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = -waypoint.Position WAYPOINT_POSITION_DOWN pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT PathDesiredSet &pathDesired pathDesired.Mode = PATHDESIRED_MODE_PATH
LibrePilot 5777 pathplanner.c Control 46fc5fa4fb Create a PathPlannerSettings object and move the PathMode setting to there.Also add a (temporary) field to allow storing some custom paths to select onthe fly. Later will be replaced with some code to fetch from the flash chip. #include pathplannersettings.h static PathPlannerSettingsData pathPlannerSettings static void settingsUpdated UAVObjEvent * ev static void createPathBox static void createPathLogo PathPlannerSettingsInitialize PathPlannerSettingsConnectCallback settingsUpdated settingsUpdated PathPlannerSettingsHandle switch pathPlannerSettings.PathMode case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: case PATHPLANNERSETTINGS_PATHMODE_PATH: switch pathPlannerSettings.PathMode case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: case PATHPLANNERSETTINGS_PATHMODE_PATH: void settingsUpdated UAVObjEvent * ev uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath PathPlannerSettingsGet &pathPlannerSettings if pathPlannerSettings.PreprogrammedPath != preprogrammedPath switch pathPlannerSettings.PreprogrammedPath case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_NONE: break case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_10M_BOX: createPathBox break case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_LOGO: createPathLogo break static void createPathBox WaypointCreateInstance WaypointCreateInstance WaypointCreateInstance WaypointCreateInstance WaypointCreateInstance WaypointData waypoint waypoint.Velocity 0 = 2 waypoint.Action = WAYPOINT_ACTION_NEXT waypoint.Position 0 = 5 waypoint.Position 1 = 5 waypoint.Position 2 = -10 WaypointInstSet 0 &waypoint waypoint.Position 0 = -5 waypoint.Position 1 = 5 WaypointInstSet 1 &waypoint waypoint.Position 0 = -5 waypoint.Position 1 = -5 WaypointInstSet 2 &waypoint waypoint.Position 0 = 5 waypoint.Position 1 = -5 WaypointInstSet 3 &waypoint waypoint.Position 0 = 5 waypoint.Position 1 = 5 WaypointInstSet 4 &waypoint waypoint.Position 0 = 0 waypoint.Position 1 = 0 WaypointInstSet 5 &waypoint static void createPathLogo
LibrePilot 5857 pathplanner.c Control 68fe938016 Implemented new programmable PathPlanner #include pathaction.h #include pathstatus.h #include velocityactual.h #define STACK_SIZE_BYTES 1024 #define F_PI 3.141526535897932f #define RAD2DEG 180.0f/F_PI static void pathPlannerTask void *parameters static void settingsUpdated UAVObjEvent * ev static void updatePathDesired UAVObjEvent * ev static void setWaypoint uint16_t num static void WaypointCreateInstances uint16_t num static void createPathBox static void createPathLogo static uint8_t pathConditionCheck static uint8_t conditionNone static uint8_t conditionTimeOut static uint8_t conditionDistanceToTarget static uint8_t conditionLegRemaining static uint8_t conditionAboveAltitude static uint8_t conditionPointingTowardsNext static uint8_t conditionPythonScript static uint8_t conditionImmediate static PathActionData pathAction static bool pathplanner_active = false PathActionInitialize PathStatusInitialize PathDesiredInitialize PositionActualInitialize VelocityActualInitialize WaypointConnectCallback updatePathDesired WaypointActiveConnectCallback updatePathDesired PathActionConnectCallback updatePathDesired PathStatusData pathStatus bool endCondition = false WaypointActiveGet &waypointActive pathplanner_active = true WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction PathStatusGet &pathStatus PathDesiredGet &pathDesired if pathStatus.UID != pathDesired.UID continue if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL setWaypoint pathAction.ErrorDestination continue endCondition = pathConditionCheck switch pathAction.Command case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if endCondition setWaypoint waypointActive.Index+1 break case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: if !endCondition setWaypoint waypointActive.Index+1 break case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: if !endCondition setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: if endCondition setWaypoint pathAction.JumpDestination else setWaypoint waypointActive.Index+1 break void updatePathDesired UAVObjEvent * ev if !pathplanner_active return static WaypointActiveData waypointActive static PathActionData pathAction static WaypointData waypoint static PathDesiredData pathDesired WaypointActiveGet &waypointActive WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction pathDesired.End PATHDESIRED_END_NORTH = waypoint.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypoint.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypoint.Position WAYPOINT_POSITION_DOWN pathDesired.EndingVelocity = waypoint.Velocity pathDesired.Mode = pathAction.Mode pathDesired.ModeParameters 0 = pathAction.ModeParameters 0 pathDesired.ModeParameters 1 = pathAction.ModeParameters 1 pathDesired.ModeParameters 2 = pathAction.ModeParameters 2 pathDesired.ModeParameters 3 = pathAction.ModeParameters 3 pathDesired.UID = waypointActive.Index if waypointActive.Index == 0 pathDesired.Start PATHDESIRED_START_NORTH = 0 pathDesired.Start PATHDESIRED_START_EAST = 0 pathDesired.Start PATHDESIRED_START_DOWN = -1 pathDesired.StartingVelocity = pathDesired.EndingVelocity else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start PATHDESIRED_END_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_END_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_END_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN pathDesired.StartingVelocity = waypointPrev.Velocity PathDesiredSet &pathDesired static void setWaypoint uint16_t num if num>=UAVObjGetNumInstances WaypointHandle num = 0 waypointActive.Index = num WaypointActiveSet &waypointActive static void WaypointCreateInstances uint16_t num uint16_t t for t=UAVObjGetNumInstances WaypointHandle t= 1e6 * pathAction.ConditionParameters 0 toWaypoint = 0xFFFF return true return false static uint8_t conditionDistanceToTarget float distance PositionActualData positionActual PositionActualGet &positionActual if pathAction.ConditionParameters 1 >0.5f distance=sqrtf powf waypoint.Position 0 -positionActual.North 2 +powf waypoint.Position 1 -positionActual.East 2 +powf waypoint.Position 1 -positionActual.Down 2 else distance=sqrtf powf waypoint.Position 0 -positionActual.North 2 +powf waypoint.Position 1 -positionActual.East 2 if distance>=pathAction.ConditionParameters 0 return true return false static uint8_t conditionLegRemaining PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress if progress.fractional_progress >= 1.0f - pathAction.ConditionParameters 0 return true return false static uint8_t conditionAboveAltitude PositionActualData positionActual PositionActualGet &positionActual if positionActual.Down <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPointingTowardsNext uint16_t nextWaypointId = waypointActive.Index+1 if nextWaypointId>=UAVObjGetNumInstances WaypointHandle nextWaypointId = 0 WaypointData nextWaypoint WaypointInstGet nextWaypointId &nextWaypoint float angle1 = atan2f nextWaypoint.Position 0 -waypoint.Position 0 nextWaypoint.Position 1 -waypoint.Position 1 VelocityActualData velocity VelocityActualGet &velocity float angle2 = atan2f velocity.North velocity.East angle1 = fabsf RAD2DEG * angle1 - angle2 while angle1>360 angle1-=360 if angle1 <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPythonScript return true static uint8_t conditionImmediate return true PathActionData action PathActionInstGet 0 &action action.Mode = PATHACTION_MODE_FLYVECTOR action.ModeParameters 0 =0 action.ModeParameters 1 =0 action.ModeParameters 2 =0 action.ModeParameters 3 =0 action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING action.ConditionParameters 0 = 0 action.ConditionParameters 1 = 0 action.ConditionParameters 2 = 0 action.ConditionParameters 3 = 0 action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT action.JumpDestination = 0 action.ErrorDestination = 0 PathActionInstSet 0 &action WaypointCreateInstances 6 waypoint.Action = 0 waypoint.Velocity = 2 PathActionData action PathActionInstGet 0 &action action.Mode = PATHACTION_MODE_FLYVECTOR action.ModeParameters 0 =0 action.ModeParameters 1 =0 action.ModeParameters 2 =0 action.ModeParameters 3 =0 action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING action.ConditionParameters 0 = 0 action.ConditionParameters 1 = 0 action.ConditionParameters 2 = 0 action.ConditionParameters 3 = 0 action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT action.JumpDestination = 0 action.ErrorDestination = 0 PathActionInstSet 0 &action WaypointCreateInstances 42 waypoint.Velocity = 2 waypoint.Action = 0
LibrePilot 5954 pathplanner.c Control ef8d7275a8 PathFollower and PathPlanner: fixes for fixed wing uint16_t t for t=UAVObjGetNumInstances PathActionHandle t<10 t++ PathActionCreateInstance PathActionInstSet 1 &action PathActionInstSet 2 &action #define SIZE 10.0f uint16_t t for t=UAVObjGetNumInstances PathActionHandle t<10 t++ PathActionCreateInstance waypoint.Position 1 = SIZE * 30 * cos i / 19.0 * 2 * M_PI waypoint.Position 0 = SIZE * 50 * sin i / 19.0 * 2 * M_PI waypoint.Position 1 = SIZE * 55 + 20 * cos i / 10.0 * M_PI - M_PI / 2 waypoint.Position 0 = SIZE * 25 + 25 * sin i / 10.0 * M_PI - M_PI / 2 waypoint.Position 1 = SIZE * 35 waypoint.Position 0 = SIZE * -50 waypoint.Position 1 = SIZE * 35 waypoint.Position 0 = SIZE * -60 waypoint.Position 1 = SIZE * 85 waypoint.Position 0 = SIZE * -60 waypoint.Position 1 = SIZE * 85 waypoint.Position 0 = SIZE * 60 waypoint.Position 1 = SIZE * -40 waypoint.Position 0 = SIZE * 60 waypoint.Position 1 = SIZE * -40 waypoint.Position 0 = SIZE * -60 waypoint.Position 1 = SIZE * 35 waypoint.Position 0 = SIZE * -60
LibrePilot 5959 pathplanner.c Control e6f4a40d3e path library: updated API and usage from pathfollowers added new conditions and additional user feedback in pathstatus #include baroairspeed.h static uint8_t conditionBelowError static uint8_t conditionAboveSpeed BaroAirspeedInitialize case PATHACTION_ENDCONDITION_BELOWERROR: return conditionBelowError break case PATHACTION_ENDCONDITION_ABOVESPEED: return conditionAboveSpeed break path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode static uint8_t conditionBelowError PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.error <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionAboveSpeed VelocityActualData velocityActual VelocityActualGet &velocityActual float velocity = sqrtf velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down if pathAction.ConditionParameters 1 >0.5f BaroAirspeedData baroAirspeed BaroAirspeedGet &baroAirspeed if baroAirspeed.Connected == BAROAIRSPEED_CONNECTED_TRUE velocity = baroAirspeed.Airspeed if velocity >= pathAction.ConditionParameters 0 return true return false
LibrePilot 5973 pathplanner.c Control 1ad2b19d28 PathPlanner: fixed a few failsafes changed waypointID to int16 - negative jump ids have special meaning if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0 case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition if pathAction.JumpDestination<0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination if pathAction.JumpDestination<0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination pathDesired.Start PATHDESIRED_START_NORTH = waypoint.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_START_EAST = waypoint.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_START_DOWN = waypoint.Position WAYPOINT_POSITION_DOWN WaypointInstSet i &waypoint WaypointInstSet i &waypoint
LibrePilot 6002 pathplanner.c Control 4ad23864c8 Treat the waypoint action as something to do once you hit that location. (e.g.PathToNext) and add a Stop value to indicate not to advance to the nextwaypoint. This is essentially a perament loiter to the pathfollower (I think)but it s really just to have the ability to delete waypoints. case WAYPOINT_ACTION_PATHTONEXT: case WAYPOINT_ACTION_STOP: break case WAYPOINT_ACTION_PATHTONEXT: case WAYPOINT_ACTION_STOP: break waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT waypoint.Action = WAYPOINT_ACTION_PATHTONEXT
LibrePilot 6003 pathplanner.c Control c892a28970 Fix firmware to work with new usage of the action waypoint field static void advanceWaypoint static void checkTerminationCondition static void activateWaypoint static int32_t active_waypoint = -1 checkTerminationCondition if active_waypoint != waypointActive.Index activateWaypoint waypointActive.Index static void checkTerminationCondition const float MIN_RADIUS = 4.0f PositionActualData positionActual switch waypoint.Action case WAYPOINT_ACTION_ENDPOINTTONEXT: PositionActualGet &positionActual float r2 = powf positionActual.North - waypoint.Position WAYPOINT_POSITION_NORTH 2 + powf positionActual.East - waypoint.Position WAYPOINT_POSITION_EAST 2 + powf positionActual.Down - waypoint.Position WAYPOINT_POSITION_DOWN 2 if r2 < MIN_RADIUS * MIN_RADIUS advanceWaypoint break case WAYPOINT_ACTION_PATHTONEXT: PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress if progress.fractional_progress >= 1 advanceWaypoint break case WAYPOINT_ACTION_STOP: break static void advanceWaypoint WaypointActiveGet &waypointActive waypointActive.Index++ WaypointActiveSet &waypointActive static void activateWaypoint int idx active_waypoint = idx uint8_t waypoint_mode = WAYPOINT_ACTION_PATHTONEXT if idx > 0 WaypointData prevWaypoint WaypointInstGet idx - 1 &prevWaypoint waypoint_mode = prevWaypoint.Action PathDesiredData pathDesired switch waypoint_mode case WAYPOINT_ACTION_RTH: PathDesiredGet &pathDesired pathDesired.End PATHDESIRED_END_NORTH = 0 pathDesired.End PATHDESIRED_END_EAST = 0 pathDesired.End PATHDESIRED_END_DOWN = -50 pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT PathDesiredSet &pathDesired break case WAYPOINT_ACTION_ENDPOINTTONEXT: WaypointInstGet idx &waypoint case WAYPOINT_ACTION_PATHTONEXT: WaypointInstGet idx &waypoint powf waypointPrev.Velocity WAYPOINT_VELOCITY_EAST 2
LibrePilot 6011 pathplanner.c Non-Control 21eb48c58c Add a RTH flight mode and remove it from the waypoint actions as that isredundant with flying to (0 0 0)
LibrePilot 6061 pathplanner.c Control ac99176f73 Add a scale to the logo float scale = 1 waypoint.Position 1 = scale * 30 * cos i / 19.0 * 2 * M_PI waypoint.Position 0 = scale * 50 * sin i / 19.0 * 2 * M_PI waypoint.Position 1 = scale * 55 + 20 * cos i / 10.0 * M_PI - M_PI / 2 waypoint.Position 0 = scale * 25 + 25 * sin i / 10.0 * M_PI - M_PI / 2 waypoint.Position 1 = scale * 35 waypoint.Position 0 = scale * -50 waypoint.Position 1 = scale * 35 waypoint.Position 0 = scale * -60 waypoint.Position 1 = scale * 85 waypoint.Position 0 = scale * -60 waypoint.Position 1 = scale * 85 waypoint.Position 0 = scale * 60 waypoint.Position 1 = scale * -40 waypoint.Position 0 = scale * 60 waypoint.Position 1 = scale * -40 waypoint.Position 0 = scale * -60 waypoint.Position 1 = scale * 35 waypoint.Position 0 = scale * -60
LibrePilot 6165 pathplanner.c Non-Control 1d976b3095 dded support for multiple airspeed sources. Added GPS airspeed estimation. Extended BaroAirspeed UAVO. Added new AirspeedSettings UAVO. if baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE
LibrePilot 6229 pathplanner.c Control aa5e5a9b01 Removed airspeed field from Airspeed UAVO. Now everything is done either with true airspeed or calibrated airspeed velocity = baroAirspeed.TrueAirspeed
LibrePilot 6255 pathplanner.c Control 24636eb5f4 Add a waypoint above home for the box pattern. waypoint.Position 0 = 0 waypoint.Position 1 = 0 waypoint.Position 2 = -10 WaypointInstSet 0 &waypoint WaypointInstSet 1 &waypoint WaypointInstSet 2 &waypoint WaypointInstSet 3 &waypoint WaypointInstSet 4 &waypoint WaypointInstSet 5 &waypoint WaypointInstSet 6 &waypoint
LibrePilot 6599 pathplanner.c Control ffaab02479 Use Correct Airspeed object in PathPlanner (and use CalibratedAirspeed since its more relevant) #include airspeedactual.h AirspeedActualInitialize AirspeedActualData airspeed AirspeedActualGet &airspeed velocity = airspeed.CalibratedAirspeed
LibrePilot 6611 pathplanner.c Non-Control 4367bd62bf OP-699 : removed unnecessary object queue in pathplanner
LibrePilot 6615 pathplanner.c Non-Control cc8e56f170 OP699: changed update rate to use constant #define PATH_PLANNER_UPDATE_RATE_MS 20 vTaskDelay PATH_PLANNER_UPDATE_RATE_MS
LibrePilot 6616 pathplanner.c Control e8232ba825 Pathplanner: removed pathPlannersettings and preprogrammed (hardcoded) paths
LibrePilot 6646 pathplanner.c Control c6effbbb0f bugfix to pathplanner if distance<=pathAction.ConditionParameters 0
LibrePilot 7342 pathplanner.c Control 5ded8c6dfc VTOL path fixesFlight mode fixesPOI modeAutoLand TestsSimple stabilization Shortest way yaw fixGCS map and pathplanner fixes PositionActualData positionActual PositionActualGet &positionActual pathDesired.Start PATHDESIRED_START_NORTH = positionActual.North pathDesired.Start PATHDESIRED_START_EAST = positionActual.East pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down pathDesired.Start PATHDESIRED_START_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_START_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_START_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN
LibrePilot 8183 pathplanner.c Control cc6d19e40a OP-908 added pios_math.h and pios_helper.h to contains all the various math and general macros/constants. Fixed remaining constants still defined as double.+review OPReview-436 if PIOS_DELAY_DiffuS toStarttime >= 1e6f * pathAction.ConditionParameters 0
LibrePilot 8992 pathplanner.c Non-Control 66db978244 Rename Libraries->libraries Modules->modules
LibrePilot 9937 pathplanner.c Non-Control 5f3e0c3e1d Math cleanup #include angle1 = fabsf RAD2DEG angle1 - angle2
LibrePilot 9948 pathplanner.c Non-Control 80c917591e Cherry-pick and hand-merge of Sami s math cleanup commit from branch sambas/diffnext: 5f3e0c3e1da9388ca7572a5cb97d67a002dc0360 #include angle1 = fabsf RAD2DEG angle1 - angle2
LibrePilot 10046 pathplanner.c Control 84e1a81f8b OP-936: Moves the task monitor code out of the flight library and into PiOS.This move and rework also breaks the dependency of the task monitor on the UAVO subsystem and pushes the responsibility for updating the TaskInfo UAVO into the System module.+review OPReview #include taskinfo.h #include paths.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_PATHPLANNER taskHandle
LibrePilot 10128 pathplanner.c Non-Control a2d8544931 OP-931: adds -Wextra compiler option for the flight code and makes the bazillion code changes requiredto make the flight code compile again. Needs careful review particularly all the fixes for thesigned vs unsigned comparisons.+review OPReview-459 static void pathPlannerTask __attribute__ unused void *parameters void updatePathDesired __attribute__ unused UAVObjEvent * ev
LibrePilot 10294 pathplanner.c Non-Control ed68fbe68d OP-951: Adds -Wshadow to flight CFLAGS and fixes resulting compilation breakage.+review OPReview static WaypointActiveData waypointActiveData static PathActionData pathActionData static WaypointData waypointData WaypointActiveGet &waypointActiveData WaypointInstGet waypointActiveData.Index &waypointData PathActionInstGet waypointData.Action &pathActionData pathDesired.End PATHDESIRED_END_NORTH = waypointData.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypointData.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypointData.Position WAYPOINT_POSITION_DOWN pathDesired.EndingVelocity = waypointData.Velocity pathDesired.Mode = pathActionData.Mode pathDesired.ModeParameters 0 = pathActionData.ModeParameters 0 pathDesired.ModeParameters 1 = pathActionData.ModeParameters 1 pathDesired.ModeParameters 2 = pathActionData.ModeParameters 2 pathDesired.ModeParameters 3 = pathActionData.ModeParameters 3 pathDesired.UID = waypointActiveData.Index if waypointActiveData.Index == 0
LibrePilot 11007 pathplanner.c Non-Control 80caceb984 make uncrustify_all;make uncrustify_all; #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY tskIDLE_PRIORITY + 1 #define MAX_QUEUE_SIZE 2 static void updatePathDesired UAVObjEvent *ev taskHandle = NULL xTaskCreate pathPlannerTask signed char * PathPlanner STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_PATHPLANNER taskHandle return 0 taskHandle = NULL PathActionInitialize PathStatusInitialize PathDesiredInitialize PositionActualInitialize AirspeedActualInitialize VelocityActualInitialize WaypointInitialize WaypointActiveInitialize return 0 WaypointConnectCallback updatePathDesired WaypointActiveConnectCallback updatePathDesired PathActionConnectCallback updatePathDesired FlightStatusData flightStatus PathDesiredData pathDesired PathStatusData pathStatus bool endCondition = false while 1 vTaskDelay PATH_PLANNER_UPDATE_RATE_MS FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER pathplanner_active = false continue WaypointActiveGet &waypointActive if pathplanner_active == false pathplanner_active = true waypointActive.Index = 0 WaypointActiveSet &waypointActive continue WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction PathStatusGet &pathStatus PathDesiredGet &pathDesired if pathStatus.UID != pathDesired.UID continue if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0 setWaypoint pathAction.ErrorDestination continue endCondition = pathConditionCheck switch pathAction.Command case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if endCondition setWaypoint waypointActive.Index + 1 break case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination else setWaypoint waypointActive.Index + 1 break void updatePathDesired __attribute__ unused UAVObjEvent *ev if !pathplanner_active return static WaypointActiveData waypointActiveData static PathActionData pathActionData static WaypointData waypointData static PathDesiredData pathDesired WaypointActiveGet &waypointActiveData WaypointInstGet waypointActiveData.Index &waypointData PathActionInstGet waypointData.Action &pathActionData pathDesired.End PATHDESIRED_END_NORTH = waypointData.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypointData.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypointData.Position WAYPOINT_POSITION_DOWN pathDesired.EndingVelocity = waypointData.Velocity pathDesired.Mode = pathActionData.Mode pathDesired.ModeParameters 0 = pathActionData.ModeParameters 0 pathDesired.ModeParameters 1 = pathActionData.ModeParameters 1 pathDesired.ModeParameters 2 = pathActionData.ModeParameters 2 pathDesired.ModeParameters 3 = pathActionData.ModeParameters 3 pathDesired.UID = waypointActiveData.Index if waypointActiveData.Index == 0 PositionActualData positionActual PositionActualGet &positionActual pathDesired.Start PATHDESIRED_START_NORTH = positionActual.North pathDesired.Start PATHDESIRED_START_EAST = positionActual.East pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start PATHDESIRED_START_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_START_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_START_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN pathDesired.StartingVelocity = waypointPrev.Velocity PathDesiredSet &pathDesired static void setWaypoint uint16_t num if num >= UAVObjGetNumInstances WaypointHandle num = 0 waypointActive.Index = num WaypointActiveSet &waypointActive static uint8_t pathConditionCheck switch pathAction.EndCondition case PATHACTION_ENDCONDITION_NONE: return conditionNone break case PATHACTION_ENDCONDITION_TIMEOUT: return conditionTimeOut break case PATHACTION_ENDCONDITION_DISTANCETOTARGET: return conditionDistanceToTarget break case PATHACTION_ENDCONDITION_LEGREMAINING: return conditionLegRemaining break case PATHACTION_ENDCONDITION_BELOWERROR: return conditionBelowError break case PATHACTION_ENDCONDITION_ABOVEALTITUDE: return conditionAboveAltitude break case PATHACTION_ENDCONDITION_ABOVESPEED: return conditionAboveSpeed break case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: return conditionPointingTowardsNext break case PATHACTION_ENDCONDITION_PYTHONSCRIPT: return conditionPythonScript break case PATHACTION_ENDCONDITION_IMMEDIATE: return conditionImmediate break default: return true break static uint8_t conditionNone return false static uint8_t conditionTimeOut static uint16_t toWaypoint static uint32_t toStarttime if waypointActive.Index != toWaypoint toWaypoint = waypointActive.Index toStarttime = PIOS_DELAY_GetRaw if PIOS_DELAY_DiffuS toStarttime >= 1e6f * pathAction.ConditionParameters 0 toWaypoint = 0xFFFF return true return false static uint8_t conditionDistanceToTarget float distance PositionActualData positionActual PositionActualGet &positionActual if pathAction.ConditionParameters 1 > 0.5f distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 + powf waypoint.Position 1 - positionActual.Down 2 else distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 if distance <= pathAction.ConditionParameters 0 return true return false * the LegRemaining measures how far the pathfollower got on a linear path segment static uint8_t conditionLegRemaining PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.fractional_progress >= 1.0f - pathAction.ConditionParameters 0 return true return false * the BelowError measures the error on a path segment static uint8_t conditionBelowError PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.error <= pathAction.ConditionParameters 0 return true return false * the AboveAltitude measures the flight altitude relative to home position static uint8_t conditionAboveAltitude PositionActualData positionActual PositionActualGet &positionActual if positionActual.Down <= pathAction.ConditionParameters 0 return true return false * the AboveSpeed measures the movement speed 3d static uint8_t conditionAboveSpeed VelocityActualData velocityActual VelocityActualGet &velocityActual float velocity = sqrtf velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down if pathAction.ConditionParameters 1 > 0.5f AirspeedActualData airspeed AirspeedActualGet &airspeed velocity = airspeed.CalibratedAirspeed if velocity >= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPointingTowardsNext uint16_t nextWaypointId = waypointActive.Index + 1 if nextWaypointId >= UAVObjGetNumInstances WaypointHandle nextWaypointId = 0 WaypointData nextWaypoint WaypointInstGet nextWaypointId &nextWaypoint float angle1 = atan2f nextWaypoint.Position 0 - waypoint.Position 0 nextWaypoint.Position 1 - waypoint.Position 1 VelocityActualData velocity VelocityActualGet &velocity float angle2 = atan2f velocity.North velocity.East angle1 = fabsf RAD2DEG angle1 - angle2 while angle1 > 360 angle1 -= 360 if angle1 <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPythonScript return true static uint8_t conditionImmediate return true
LibrePilot 11220 pathplanner.c Non-Control 5284195c29 Refaktored sensor and state UAVObjects consistently into XXYSensor and XXYState #include airspeedstate.h #include positionstate.h #include velocitystate.h PositionStateInitialize AirspeedStateInitialize VelocityStateInitialize PositionStateData positionState PositionStateGet &positionState pathDesired.Start PATHDESIRED_START_NORTH = positionState.North pathDesired.Start PATHDESIRED_START_EAST = positionState.East pathDesired.Start PATHDESIRED_START_DOWN = positionState.Down PositionStateData positionState PositionStateGet &positionState distance = sqrtf powf waypoint.Position 0 - positionState.North 2 + powf waypoint.Position 1 - positionState.East 2 + powf waypoint.Position 1 - positionState.Down 2 distance = sqrtf powf waypoint.Position 0 - positionState.North 2 + powf waypoint.Position 1 - positionState.East 2 PositionStateData positionState PositionStateGet &positionState float cur 3 = positionState.North positionState.East positionState.Down PositionStateData positionState PositionStateGet &positionState float cur 3 = positionState.North positionState.East positionState.Down PositionStateData positionState PositionStateGet &positionState if positionState.Down <= pathAction.ConditionParameters 0 VelocityStateData velocityState VelocityStateGet &velocityState float velocity = sqrtf velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down AirspeedStateData airspeed AirspeedStateGet &airspeed VelocityStateData velocity VelocityStateGet &velocity
LibrePilot 11592 pathplanner.c Non-Control daf329d8f5 uncrustification (again) #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY tskIDLE_PRIORITY + 1 #define MAX_QUEUE_SIZE 2 static void updatePathDesired UAVObjEvent *ev taskHandle = NULL xTaskCreate pathPlannerTask signed char * PathPlanner STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_PATHPLANNER taskHandle return 0 taskHandle = NULL PathActionInitialize PathStatusInitialize PathDesiredInitialize PositionActualInitialize AirspeedActualInitialize VelocityActualInitialize WaypointInitialize WaypointActiveInitialize return 0 WaypointConnectCallback updatePathDesired WaypointActiveConnectCallback updatePathDesired PathActionConnectCallback updatePathDesired FlightStatusData flightStatus PathDesiredData pathDesired PathStatusData pathStatus bool endCondition = false while 1 vTaskDelay PATH_PLANNER_UPDATE_RATE_MS FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER pathplanner_active = false continue WaypointActiveGet &waypointActive if pathplanner_active == false pathplanner_active = true waypointActive.Index = 0 WaypointActiveSet &waypointActive continue WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction PathStatusGet &pathStatus PathDesiredGet &pathDesired if pathStatus.UID != pathDesired.UID continue if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0 setWaypoint pathAction.ErrorDestination continue endCondition = pathConditionCheck switch pathAction.Command case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if endCondition setWaypoint waypointActive.Index + 1 break case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination else setWaypoint waypointActive.Index + 1 break void updatePathDesired __attribute__ unused UAVObjEvent *ev if !pathplanner_active return static WaypointActiveData waypointActiveData static PathActionData pathActionData static WaypointData waypointData static PathDesiredData pathDesired WaypointActiveGet &waypointActiveData WaypointInstGet waypointActiveData.Index &waypointData PathActionInstGet waypointData.Action &pathActionData pathDesired.End PATHDESIRED_END_NORTH = waypointData.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypointData.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypointData.Position WAYPOINT_POSITION_DOWN pathDesired.EndingVelocity = waypointData.Velocity pathDesired.Mode = pathActionData.Mode pathDesired.ModeParameters 0 = pathActionData.ModeParameters 0 pathDesired.ModeParameters 1 = pathActionData.ModeParameters 1 pathDesired.ModeParameters 2 = pathActionData.ModeParameters 2 pathDesired.ModeParameters 3 = pathActionData.ModeParameters 3 pathDesired.UID = waypointActiveData.Index if waypointActiveData.Index == 0 PositionActualData positionActual PositionActualGet &positionActual pathDesired.Start PATHDESIRED_START_NORTH = positionActual.North pathDesired.Start PATHDESIRED_START_EAST = positionActual.East pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start PATHDESIRED_START_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_START_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_START_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN pathDesired.StartingVelocity = waypointPrev.Velocity PathDesiredSet &pathDesired static void setWaypoint uint16_t num if num >= UAVObjGetNumInstances WaypointHandle num = 0 waypointActive.Index = num WaypointActiveSet &waypointActive static uint8_t pathConditionCheck switch pathAction.EndCondition case PATHACTION_ENDCONDITION_NONE: return conditionNone break case PATHACTION_ENDCONDITION_TIMEOUT: return conditionTimeOut break case PATHACTION_ENDCONDITION_DISTANCETOTARGET: return conditionDistanceToTarget break case PATHACTION_ENDCONDITION_LEGREMAINING: return conditionLegRemaining break case PATHACTION_ENDCONDITION_BELOWERROR: return conditionBelowError break case PATHACTION_ENDCONDITION_ABOVEALTITUDE: return conditionAboveAltitude break case PATHACTION_ENDCONDITION_ABOVESPEED: return conditionAboveSpeed break case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: return conditionPointingTowardsNext break case PATHACTION_ENDCONDITION_PYTHONSCRIPT: return conditionPythonScript break case PATHACTION_ENDCONDITION_IMMEDIATE: return conditionImmediate break default: return true break static uint8_t conditionNone return false static uint8_t conditionTimeOut static uint16_t toWaypoint static uint32_t toStarttime if waypointActive.Index != toWaypoint toWaypoint = waypointActive.Index toStarttime = PIOS_DELAY_GetRaw if PIOS_DELAY_DiffuS toStarttime >= 1e6f * pathAction.ConditionParameters 0 toWaypoint = 0xFFFF return true return false static uint8_t conditionDistanceToTarget float distance PositionActualData positionActual PositionActualGet &positionActual if pathAction.ConditionParameters 1 > 0.5f distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 + powf waypoint.Position 1 - positionActual.Down 2 else distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 if distance <= pathAction.ConditionParameters 0 return true return false * the LegRemaining measures how far the pathfollower got on a linear path segment static uint8_t conditionLegRemaining PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.fractional_progress >= 1.0f - pathAction.ConditionParameters 0 return true return false * the BelowError measures the error on a path segment static uint8_t conditionBelowError PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.error <= pathAction.ConditionParameters 0 return true return false * the AboveAltitude measures the flight altitude relative to home position static uint8_t conditionAboveAltitude PositionActualData positionActual PositionActualGet &positionActual if positionActual.Down <= pathAction.ConditionParameters 0 return true return false * the AboveSpeed measures the movement speed 3d static uint8_t conditionAboveSpeed VelocityActualData velocityActual VelocityActualGet &velocityActual float velocity = sqrtf velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down if pathAction.ConditionParameters 1 > 0.5f AirspeedActualData airspeed AirspeedActualGet &airspeed velocity = airspeed.CalibratedAirspeed if velocity >= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPointingTowardsNext uint16_t nextWaypointId = waypointActive.Index + 1 if nextWaypointId >= UAVObjGetNumInstances WaypointHandle nextWaypointId = 0 WaypointData nextWaypoint WaypointInstGet nextWaypointId &nextWaypoint float angle1 = atan2f nextWaypoint.Position 0 - waypoint.Position 0 nextWaypoint.Position 1 - waypoint.Position 1 VelocityActualData velocity VelocityActualGet &velocity float angle2 = atan2f velocity.North velocity.East angle1 = fabsf RAD2DEG angle1 - angle2 while angle1 > 360 angle1 -= 360 if angle1 <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPythonScript return true static uint8_t conditionImmediate return true
LibrePilot 11851 pathplanner.c Non-Control 900f643bbd Reformat source code with make uncrustify_all run twice. NO CODE CHANGES #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY tskIDLE_PRIORITY + 1 #define MAX_QUEUE_SIZE 2 static void updatePathDesired UAVObjEvent *ev taskHandle = NULL xTaskCreate pathPlannerTask signed char * PathPlanner STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_PATHPLANNER taskHandle return 0 taskHandle = NULL PathActionInitialize PathStatusInitialize PathDesiredInitialize PositionActualInitialize AirspeedActualInitialize VelocityActualInitialize WaypointInitialize WaypointActiveInitialize return 0 WaypointConnectCallback updatePathDesired WaypointActiveConnectCallback updatePathDesired PathActionConnectCallback updatePathDesired FlightStatusData flightStatus PathDesiredData pathDesired PathStatusData pathStatus bool endCondition = false while 1 vTaskDelay PATH_PLANNER_UPDATE_RATE_MS FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER pathplanner_active = false continue WaypointActiveGet &waypointActive if pathplanner_active == false pathplanner_active = true waypointActive.Index = 0 WaypointActiveSet &waypointActive continue WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction PathStatusGet &pathStatus PathDesiredGet &pathDesired if pathStatus.UID != pathDesired.UID continue if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0 setWaypoint pathAction.ErrorDestination continue endCondition = pathConditionCheck switch pathAction.Command case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if endCondition setWaypoint waypointActive.Index + 1 break case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination else setWaypoint waypointActive.Index + 1 break void updatePathDesired __attribute__ unused UAVObjEvent *ev if !pathplanner_active return static WaypointActiveData waypointActiveData static PathActionData pathActionData static WaypointData waypointData static PathDesiredData pathDesired WaypointActiveGet &waypointActiveData WaypointInstGet waypointActiveData.Index &waypointData PathActionInstGet waypointData.Action &pathActionData pathDesired.End PATHDESIRED_END_NORTH = waypointData.Position WAYPOINT_POSITION_NORTH pathDesired.End PATHDESIRED_END_EAST = waypointData.Position WAYPOINT_POSITION_EAST pathDesired.End PATHDESIRED_END_DOWN = waypointData.Position WAYPOINT_POSITION_DOWN pathDesired.EndingVelocity = waypointData.Velocity pathDesired.Mode = pathActionData.Mode pathDesired.ModeParameters 0 = pathActionData.ModeParameters 0 pathDesired.ModeParameters 1 = pathActionData.ModeParameters 1 pathDesired.ModeParameters 2 = pathActionData.ModeParameters 2 pathDesired.ModeParameters 3 = pathActionData.ModeParameters 3 pathDesired.UID = waypointActiveData.Index if waypointActiveData.Index == 0 PositionActualData positionActual PositionActualGet &positionActual pathDesired.Start PATHDESIRED_START_NORTH = positionActual.North pathDesired.Start PATHDESIRED_START_EAST = positionActual.East pathDesired.Start PATHDESIRED_START_DOWN = positionActual.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start PATHDESIRED_START_NORTH = waypointPrev.Position WAYPOINT_POSITION_NORTH pathDesired.Start PATHDESIRED_START_EAST = waypointPrev.Position WAYPOINT_POSITION_EAST pathDesired.Start PATHDESIRED_START_DOWN = waypointPrev.Position WAYPOINT_POSITION_DOWN pathDesired.StartingVelocity = waypointPrev.Velocity PathDesiredSet &pathDesired static void setWaypoint uint16_t num if num >= UAVObjGetNumInstances WaypointHandle num = 0 waypointActive.Index = num WaypointActiveSet &waypointActive static uint8_t pathConditionCheck switch pathAction.EndCondition case PATHACTION_ENDCONDITION_NONE: return conditionNone break case PATHACTION_ENDCONDITION_TIMEOUT: return conditionTimeOut break case PATHACTION_ENDCONDITION_DISTANCETOTARGET: return conditionDistanceToTarget break case PATHACTION_ENDCONDITION_LEGREMAINING: return conditionLegRemaining break case PATHACTION_ENDCONDITION_BELOWERROR: return conditionBelowError break case PATHACTION_ENDCONDITION_ABOVEALTITUDE: return conditionAboveAltitude break case PATHACTION_ENDCONDITION_ABOVESPEED: return conditionAboveSpeed break case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: return conditionPointingTowardsNext break case PATHACTION_ENDCONDITION_PYTHONSCRIPT: return conditionPythonScript break case PATHACTION_ENDCONDITION_IMMEDIATE: return conditionImmediate break default: return true break static uint8_t conditionNone return false static uint8_t conditionTimeOut static uint16_t toWaypoint static uint32_t toStarttime if waypointActive.Index != toWaypoint toWaypoint = waypointActive.Index toStarttime = PIOS_DELAY_GetRaw if PIOS_DELAY_DiffuS toStarttime >= 1e6f * pathAction.ConditionParameters 0 toWaypoint = 0xFFFF return true return false static uint8_t conditionDistanceToTarget float distance PositionActualData positionActual PositionActualGet &positionActual if pathAction.ConditionParameters 1 > 0.5f distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 + powf waypoint.Position 1 - positionActual.Down 2 else distance = sqrtf powf waypoint.Position 0 - positionActual.North 2 + powf waypoint.Position 1 - positionActual.East 2 if distance <= pathAction.ConditionParameters 0 return true return false * the LegRemaining measures how far the pathfollower got on a linear path segment static uint8_t conditionLegRemaining PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.fractional_progress >= 1.0f - pathAction.ConditionParameters 0 return true return false * the BelowError measures the error on a path segment static uint8_t conditionBelowError PathDesiredData pathDesired PositionActualData positionActual PathDesiredGet &pathDesired PositionActualGet &positionActual float cur 3 = positionActual.North positionActual.East positionActual.Down struct path_status progress path_progress pathDesired.Start pathDesired.End cur &progress pathDesired.Mode if progress.error <= pathAction.ConditionParameters 0 return true return false * the AboveAltitude measures the flight altitude relative to home position static uint8_t conditionAboveAltitude PositionActualData positionActual PositionActualGet &positionActual if positionActual.Down <= pathAction.ConditionParameters 0 return true return false * the AboveSpeed measures the movement speed 3d static uint8_t conditionAboveSpeed VelocityActualData velocityActual VelocityActualGet &velocityActual float velocity = sqrtf velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down if pathAction.ConditionParameters 1 > 0.5f AirspeedActualData airspeed AirspeedActualGet &airspeed velocity = airspeed.CalibratedAirspeed if velocity >= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPointingTowardsNext uint16_t nextWaypointId = waypointActive.Index + 1 if nextWaypointId >= UAVObjGetNumInstances WaypointHandle nextWaypointId = 0 WaypointData nextWaypoint WaypointInstGet nextWaypointId &nextWaypoint float angle1 = atan2f nextWaypoint.Position 0 - waypoint.Position 0 nextWaypoint.Position 1 - waypoint.Position 1 VelocityActualData velocity VelocityActualGet &velocity float angle2 = atan2f velocity.North velocity.East angle1 = fabsf RAD2DEG angle1 - angle2 while angle1 > 360 angle1 -= 360 if angle1 <= pathAction.ConditionParameters 0 return true return false static uint8_t conditionPythonScript return true static uint8_t conditionImmediate return true
LibrePilot 12186 pathplanner.c Non-Control 23b2907d08 OP-976: Add the ; backThis compile successfuly with make all_flight MODULE_INITCALL PathPlannerInitialize PathPlannerStart
LibrePilot 12361 pathplanner.c Control 0ffb2dee02 bugfix in pathplanner - 3d distance not calculated correctly + powf waypoint.Position 2 - positionState.Down 2
LibrePilot 12451 pathplanner.c Non-Control 9e1acc3165 OP-1058: fix needed for fw_revolution code compilation pathDesired.End.fields.North = waypointData.Position.fields.North pathDesired.End.fields.East = waypointData.Position.fields.East pathDesired.End.fields.Down = waypointData.Position.fields.Down pathDesired.Start.fields.North = positionState.North pathDesired.Start.fields.East = positionState.East pathDesired.Start.fields.Down = positionState.Down pathDesired.Start.fields.North = waypointPrev.Position.fields.North pathDesired.Start.fields.East = waypointPrev.Position.fields.East pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down distance = sqrtf powf waypoint.Position.fields.North - positionState.North 2 + powf waypoint.Position.fields.East - positionState.East 2 + powf waypoint.Position.fields.Down - positionState.Down 2 distance = sqrtf powf waypoint.Position.fields.North - positionState.North 2 + powf waypoint.Position.fields.East - positionState.East 2 path_progress pathDesired.Start.data pathDesired.End.data cur &progress pathDesired.Mode path_progress pathDesired.Start.data pathDesired.End.data cur &progress pathDesired.Mode float angle1 = atan2f nextWaypoint.Position.fields.North - waypoint.Position.fields.North nextWaypoint.Position.fields.East - waypoint.Position.fields.East
LibrePilot 12468 pathplanner.c Non-Control 01d963affd OP-1058 uncrustify pathDesired.End.fields.North = waypointData.Position.fields.North pathDesired.End.fields.East = waypointData.Position.fields.East pathDesired.End.fields.Down = waypointData.Position.fields.Down pathDesired.Start.fields.East = positionState.East pathDesired.Start.fields.Down = positionState.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity pathDesired.Start.fields.East = waypointPrev.Position.fields.East pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down pathDesired.StartingVelocity = waypointPrev.Velocity
LibrePilot 12491 pathplanner.c Non-Control e91bc28667 OP-1058 Get rid of Unions.They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions.It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays+review OPReview-552 #include pathDesired.End.North = waypointData.Position.North pathDesired.End.East = waypointData.Position.East pathDesired.End.Down = waypointData.Position.Down pathDesired.Start.North = positionState.North pathDesired.Start.East = positionState.East pathDesired.Start.Down = positionState.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity pathDesired.Start.North = waypointPrev.Position.North pathDesired.Start.East = waypointPrev.Position.East pathDesired.Start.Down = waypointPrev.Position.Down pathDesired.StartingVelocity = waypointPrev.Velocity distance = sqrtf powf waypoint.Position.North - positionState.North 2 + powf waypoint.Position.East - positionState.East 2 + powf waypoint.Position.Down - positionState.Down 2 distance = sqrtf powf waypoint.Position.North - positionState.North 2 + powf waypoint.Position.East - positionState.East 2 path_progress cast_struct_to_array pathDesired.Start pathDesired.Start.North cast_struct_to_array pathDesired.End pathDesired.End.North cur &progress pathDesired.Mode path_progress cast_struct_to_array pathDesired.Start pathDesired.Start.North cast_struct_to_array pathDesired.End pathDesired.End.North cur &progress pathDesired.Mode float angle1 = atan2f nextWaypoint.Position.North - waypoint.Position.North nextWaypoint.Position.East - waypoint.Position.East
LibrePilot 12663 pathplanner.c Control 8c7792b6e5 OP-1122 OP-1120 Added a new FligthPlanInfo uavobject- FligthPlanInfo contains the number of waypoints and action pathes- FligthPlanInfo contains a CRC of all waypoints and action pathes- The CRC is not yet implemented- These informations will be used to check the consistency of a flight plan #include flightplaninfo.h FlightPlanInfoInitialize
LibrePilot 12675 pathplanner.c Non-Control aa24f2193a OP-1122 OP-1158 renamed FlightPlanInfo uavobject to FlightPlan #include flightplan.h FlightPlanInitialize
LibrePilot 12677 pathplanner.c Control a64720a9f1 OP-1122 OP-1158 added flight plan CRC consistency checks (in GCS and fixedwingpathfollower module)- need to do the same in vtolpathfollower module)- addressed an issue that could cause unwanted waypoints to be used (path follower was looping over all instanciated wp) FlightPlanData flightPlan FlightPlanGet &flightPlan if num >= flightPlan.WaypointCount
LibrePilot 12679 pathplanner.c Control b8118f51e7 OP-1156 Made PathPlanner work with delayed callbacks in navigation callback taskConflicts:flight/modules/PathPlanner/pathplanner.c #define TASK_PRIORITY CALLBACK_TASK_NAVIGATION #define PATH_PLANNER_UPDATE_RATE_MS 100 static void pathPlannerTask static void commandUpdated UAVObjEvent *ev static void statusUpdated UAVObjEvent *ev static void updatePathDesired static DelayedCallbackInfo *pathPlannerHandle static DelayedCallbackInfo *pathDesiredUpdaterHandle WaypointConnectCallback commandUpdated WaypointActiveConnectCallback commandUpdated PathActionConnectCallback commandUpdated PathStatusConnectCallback statusUpdated DelayedCallbackDispatch pathPlannerHandle pathPlannerHandle = DelayedCallbackCreate &pathPlannerTask CALLBACK_PRIORITY_REGULAR TASK_PRIORITY STACK_SIZE_BYTES pathDesiredUpdaterHandle = DelayedCallbackCreate &updatePathDesired CALLBACK_PRIORITY_CRITICAL TASK_PRIORITY STACK_SIZE_BYTES static void pathPlannerTask DelayedCallbackSchedule pathPlannerHandle PATH_PLANNER_UPDATE_RATE_MS CALLBACK_UPDATEMODE_SOONER FlightStatusData flightStatus FlightStatusGet &flightStatus if flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER pathplanner_active = false return WaypointActiveGet &waypointActive if pathplanner_active == false pathplanner_active = true waypointActive.Index = 0 WaypointActiveSet &waypointActive return WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction PathStatusData pathStatus PathStatusGet &pathStatus PathDesiredData pathDesired PathDesiredGet &pathDesired if pathStatus.UID != pathDesired.UID return if pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0 setWaypoint pathAction.ErrorDestination return endCondition = pathConditionCheck switch pathAction.Command case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if endCondition setWaypoint waypointActive.Index + 1 break case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: endCondition = !endCondition case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination else setWaypoint pathAction.JumpDestination break case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: if endCondition if pathAction.JumpDestination < 0 setWaypoint waypointActive.Index - pathAction.JumpDestination setWaypoint pathAction.JumpDestination else setWaypoint waypointActive.Index + 1 break void commandUpdated __attribute__ unused UAVObjEvent *ev DelayedCallbackDispatch pathDesiredUpdaterHandle void statusUpdated __attribute__ unused UAVObjEvent *ev DelayedCallbackDispatch pathPlannerHandle void updatePathDesired PathDesiredData pathDesired WaypointActiveGet &waypointActive WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction pathDesired.End.North = waypoint.Position.North pathDesired.End.East = waypoint.Position.East pathDesired.End.Down = waypoint.Position.Down pathDesired.EndingVelocity = waypoint.Velocity pathDesired.Mode = pathAction.Mode pathDesired.ModeParameters 0 = pathAction.ModeParameters 0 pathDesired.ModeParameters 1 = pathAction.ModeParameters 1 pathDesired.ModeParameters 2 = pathAction.ModeParameters 2 pathDesired.ModeParameters 3 = pathAction.ModeParameters 3 pathDesired.UID = waypointActive.Index if waypointActive.Index == 0
LibrePilot 12680 pathplanner.c Control cb9e76b006 OP-1122 add flightplan safety check to pathplanner - failsafe rth when flightmode is set to pathplan but no valid plan is uploaded #include manualcontrolsettings.h static uint8_t checkFlightPlan uint8_t validFlightPlan = checkFlightPlan if !validFlightPlan AlarmsSet SYSTEMALARMS_ALARM_PATHPLAN SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_PATHPLAN PathDesiredData pathDesired PathDesiredGet &pathDesired static uint8_t failsafeRTHset = 0 if !validFlightPlan pathplanner_active = false if !failsafeRTHset failsafeRTHset = 1 PositionStateData positionState PositionStateGet &positionState ManualControlSettingsData settings ManualControlSettingsGet &settings pathDesired.Start.North = 0 pathDesired.Start.East = 0 pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset pathDesired.End.North = 0 pathDesired.End.East = 0 pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset pathDesired.StartingVelocity = 1 pathDesired.EndingVelocity = 0 pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT PathDesiredSet &pathDesired failsafeRTHset = 0 static uint8_t checkFlightPlan uint16_t i uint16_t waypointCount uint16_t actionCount uint8_t flightCrc FlightPlanData flightPlan FlightPlanGet &flightPlan waypointCount = flightPlan.WaypointCount actionCount = flightPlan.PathActionCount if waypointCount > UAVObjGetNumInstances WaypointHandle PIOS_DEBUGLOG_Printf FlighPlan : waypoint count error! return false if actionCount > UAVObjGetNumInstances PathActionHandle PIOS_DEBUGLOG_Printf FlighPlan : path action count error! return false flightCrc = 0 for i = 0 i < waypointCount i++ flightCrc = UAVObjUpdateCRC WaypointHandle i flightCrc for i = 0 i < actionCount i++ flightCrc = UAVObjUpdateCRC PathActionHandle i flightCrc if flightCrc != flightPlan.Crc PIOS_DEBUGLOG_Printf FlighPlan : bad CRC %d / %d ! flightCrc flightPlan.Crc return false PIOS_DEBUGLOG_Printf FlighPlan : passed consistency check. flightCrc flightPlan.Crc return true
LibrePilot 12682 pathplanner.c Non-Control 76ee48bc44 OP-1122 safer sanity checks and arming prevention in case of invalid flight plan if !validFlightPlan AlarmsSet SYSTEMALARMS_ALARM_PATHPLAN SYSTEMALARMS_ALARM_WARNING else AlarmsClear SYSTEMALARMS_ALARM_PATHPLAN AlarmsSet SYSTEMALARMS_ALARM_PATHPLAN SYSTEMALARMS_ALARM_ERROR return AlarmsClear SYSTEMALARMS_ALARM_PATHPLAN
LibrePilot 12692 pathplanner.c Non-Control 7f2c24db2b OP-1122 OP-1158 - renamed uavobject FlightPlan to PathPlan+review OPReview-609 #include pathplan.h static uint8_t checkPathPlan PathPlanInitialize uint8_t validPathPlan = checkPathPlan if !validPathPlan if !validPathPlan static uint8_t checkPathPlan uint8_t pathCrc PathPlanData pathPlan PathPlanGet &pathPlan waypointCount = pathPlan.WaypointCount actionCount = pathPlan.PathActionCount pathCrc = 0 pathCrc = UAVObjUpdateCRC WaypointHandle i pathCrc pathCrc = UAVObjUpdateCRC PathActionHandle i pathCrc if pathCrc != pathPlan.Crc PIOS_DEBUGLOG_Printf FlighPlan : bad CRC %d / %d ! pathCrc pathPlan.Crc PIOS_DEBUGLOG_Printf FlighPlan : passed consistency check. pathCrc pathPlan.Crc PathPlanData pathPlan PathPlanGet &pathPlan if num >= pathPlan.WaypointCount
LibrePilot 12694 pathplanner.c Non-Control 8ccdb252d1 OP-1122 OP-1145 commented out PIOS_DEBUGLOG_Printf calls in pathplanner+review OPReview-609
LibrePilot 12695 pathplanner.c Control 1fca85784c OP-1122 OP-1145 added more path plan checks :- empty plan (i.e. no waypoints defined)- out of range waypoint and path action ids+review OPReview-609 if waypointCount == 0 return false for i = 0 i < waypointCount i++ WaypointInstGet i &waypoint if waypoint.Action >= actionCount return false for i = 0 i < actionCount i++ PathActionInstGet i &pathAction if pathAction.ErrorDestination >= waypointCount return false if pathAction.JumpDestination >= waypointCount return false
LibrePilot 12699 pathplanner.c Non-Control d43b220dc0 OP-1122 uncrustified gcs and flight actionCount = pathPlan.PathActionCount
LibrePilot 12733 pathplanner.c Non-Control b076ba02a1 OP-942 API change to CallbackScheduler - include unique ID for each callback for a taskinfo like UAVObject #include callbackinfo.h pathPlannerHandle = DelayedCallbackCreate &pathPlannerTask CALLBACK_PRIORITY_REGULAR TASK_PRIORITY CALLBACKINFO_RUNNING_PATHPLANNER0 STACK_SIZE_BYTES pathDesiredUpdaterHandle = DelayedCallbackCreate &updatePathDesired CALLBACK_PRIORITY_CRITICAL TASK_PRIORITY CALLBACKINFO_RUNNING_PATHPLANNER1 STACK_SIZE_BYTES
LibrePilot 12743 pathplanner.c Non-Control 49a1c4c28f OP-942 refactored callback scheduler to fit into PiOS naming scheme PIOS_CALLBACKSCHEDULER_Dispatch pathPlannerHandle pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create &pathPlannerTask CALLBACK_PRIORITY_REGULAR TASK_PRIORITY CALLBACKINFO_RUNNING_PATHPLANNER0 STACK_SIZE_BYTES pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create &updatePathDesired CALLBACK_PRIORITY_CRITICAL TASK_PRIORITY CALLBACKINFO_RUNNING_PATHPLANNER1 STACK_SIZE_BYTES PIOS_CALLBACKSCHEDULER_Schedule pathPlannerHandle PATH_PLANNER_UPDATE_RATE_MS CALLBACK_UPDATEMODE_SOONER PIOS_CALLBACKSCHEDULER_Dispatch pathDesiredUpdaterHandle PIOS_CALLBACKSCHEDULER_Dispatch pathPlannerHandle
LibrePilot 12795 pathplanner.c Non-Control b5a27d05e7 OP-1197 configurable failsafes OP-1219 thrust control OP-1217 separate manualcontrolsettings apart #include flightmodesettings.h FlightModeSettingsData settings FlightModeSettingsGet &settings
LibrePilot 12815 pathplanner.c Control 7dd911efdb OP-1217 change the way modules are enabled/disabled based on flightstatus if flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE
LibrePilot 13044 pathplanner.c Non-Control 6d3ade4947 OP-1308 Set the same logic for alarms AlarmsSet SYSTEMALARMS_ALARM_PATHPLAN SYSTEMALARMS_ALARM_CRITICAL
LibrePilot 13139 pathplanner.c Non-Control 80bd04075f OP-1350 Rename ReturnToHomeAltitudeOffset to ReturnToBaseAltitudeOffset pathDesired.Start.Down = positionState.Down - settings.ReturnToBaseAltitudeOffset pathDesired.End.Down = positionState.Down - settings.ReturnToBaseAltitudeOffset
LibrePilot 13195 pathplanner.c Non-Control 92bf876654 OP-1350 refactor into a plans library all functions dealing with initialization and execution of PathFollower/pathplanner commands for manualcontrol #include plans.h plan_initialize plan_setup_positionHold
LibrePilot 13461 pathplanner.c Control b562fcb02e OP-1156 fix path logic to not deviate from correct altitude too much path_progress &pathDesired cur &progress path_progress &pathDesired cur &progress
LibrePilot 13517 pathplanner.c Non-Control 6faf1a73ac OP-1456 get rid of cast_struct_to_array because it is error prone!
LibrePilot 14127 pathplanner.c Control 88494e2745 OP-1696 PathFollower rewrite in C++ and Landing Squashed commit of the following:commit 580cd8addafadc3109b90b1fd183130ea8add7f2Author: abeck70 Date: Fri Mar 20 21:32:33 2015 +1100 OP-1696 Cleanup and bug fix 1. Code review cleanup 2. Fixed gps assist braking - the thrust limits in piddown were not being set.commit 31ac4494f6e2bc5859f49d1f76f19fe3a4d32862Author: abeck70 Date: Wed Mar 18 08:31:16 2015 +1100 OP-1696 fix initialisation of down pid control parameterscommit a7bd737459249530bde9546b8247dd45b40dced4Author: abeck70 Date: Tue Mar 17 21:18:38 2015 +1100 OP-1769 Actuator fix from code reviewcommit 968172b8fb46c91b2611be4825e17faf2fa13411Author: abeck70 Date: Tue Mar 17 21:01:11 2015 +1100 OP-1696 fixed ground pathfollower settings xmlcommit 721c07b67a87911ed67d0b62aa658cfac6d3eb97Merge: f556215 be9f4aeAuthor: abeck70 Date: Tue Mar 17 20:52:01 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit f556215ddbd5a29b22c2e6124372d74a0d6b9a0fAuthor: abeck70 Date: Tue Mar 17 20:51:22 2015 +1100 OP-1696 ground pathfollower fixescommit 4584b22d6a2d33d3efb1cd11ac46804a5623a236Author: abeck70 Date: Tue Mar 17 18:11:52 2015 +1100 OP-1696 Ground redesign 1. Move to new PID loop design 2. Prototype steering controlcommit b70c3373646fa758ae30a59765b8a56df7d2fbebAuthor: abeck70 Date: Sun Mar 15 18:42:06 2015 +1100 OP-1696 ground pathfollower added missing call to update velocity statecommit a282bfb14dffcd486466b60be3100d26c11649efAuthor: abeck70 Date: Sun Mar 15 17:22:02 2015 +1100 OP-1696 ground pathfollower improvementscommit f56ea4f9b101415e32e4464e51e087ee350be969Author: abeck70 Date: Sun Mar 15 07:44:01 2015 +1100 OP-1696 remove unused PID controller in ground for verticalcommit 6424aa0a84c3718a5a7b51fab1c9e04587d325b8Author: abeck70 Date: Sat Mar 14 21:40:58 2015 +1100 OP-1696 Initialise vtolemergencyfallback variablescommit 5d6110499bb06fe9b397e09dea5aafe994f03d13Author: abeck70 Date: Sat Mar 14 21:30:21 2015 +1100 OP-1696 vtolflycontroller segv fixed and uncrustifycommit bef23ea3dc2b5aff5b9b8713802586719b8f68cfAuthor: abeck70 Date: Sat Mar 14 20:50:05 2015 +1100 OP-1783 PathPlanner Instead of config alarm if no pathplan simply continue to alarm pathplan but allow normal arming and flight in other modes. If pathplan enabled simply enter position hold.commit 370f1747209cca7c01c071a43224ad83b04bdd55Author: abeck70 Date: Sat Mar 14 20:48:22 2015 +1100 OP-1769 Ground code review comment 1. Remove idx1 < 0 check not needed 2. Should return abs_input in the corner case that curve is invalid .commit 01973c656d783e035ad823c267d3387bf473eebeMerge: be07d12 5c9b6faAuthor: abeck70 Date: Sat Mar 14 12:17:31 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit be07d125f42b5ca1aabce0f5c0fcf592e8cddf98Merge: 44afdc7 ad44b31Author: abeck70 Date: Thu Mar 12 21:55:52 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 44afdc77987049dc58cdf42e3be88b7eae38de02Author: abeck70 Date: Thu Mar 12 19:35:55 2015 +1100 OP-1696 fix startup Fix initialisation and void use of pios_malloc at startupcommit 962f5f75756883cec6726c0347310d60264f6989Author: abeck70 Date: Thu Mar 12 18:48:04 2015 +1100 OP-1696 undo changes to make filecommit 7fa71d5c614d492cb4026312672068f92a4bcd3eMerge: 672732e 5a9cfa6Author: abeck70 Date: Thu Mar 12 18:06:59 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 672732e95d555946300ef5093f687f3fa88bfd3aAuthor: abeck70 Date: Wed Mar 11 12:59:44 2015 +1100 OP-1696 code review commentscommit 73b295d6515f2489b3b3a61459c975b0c37afe3dAuthor: abeck70 Date: Wed Mar 11 12:47:24 2015 +1100 OP-1696 code review commentscommit 524b72f537fe922bf3c4385b418872f13819a5cfMerge: 6beef01 ba5f395Author: abeck70 Date: Tue Mar 10 23:19:06 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 6beef01476d06598652a10de4857e6ae55015417Merge: 100cf33 8c31f96Author: abeck70 Date: Sat Mar 7 16:50:32 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 100cf335d187fe696d3678371b194fc3044a1c7eAuthor: Alex Beck Date: Thu Mar 5 22:32:30 2015 +1100 OP-1696 simposix c++ compilation fixcommit 9ebf846d1ec8fef30ffc1204546fc5dc648ec10bAuthor: Alex Beck Date: Thu Mar 5 21:35:39 2015 +1100 OP-1696 fix simposix c++ compile of pathfollowercommit a86b4b1db46dba569739587ee57b03460381ebabAuthor: abeck70 Date: Thu Mar 5 18:03:51 2015 +1100 OP-1696 resolve linux build issuecommit 1d7695f81293534bc17f69b0fcd9de3876867f6aAuthor: abeck70 Date: Wed Mar 4 18:10:09 2015 +1100 OP-1696 Return to Base & Land implemented Enabled via FlightModeSettings ReturnToBaseNextCommand = LANDcommit 4e5ca807b18a40ea60c05c1b46f738462595a6b2Author: abeck70 Date: Wed Mar 4 16:38:06 2015 +1100 OP-1696 reduce memory footprint by only allocating controllers required for a particular frame typecommit 78e8f559cd25ffaeac68b563e25d1ad357205fd6Author: abeck70 Date: Wed Mar 4 13:51:05 2015 +1100 OP-1696 move fsm initialisation to controller implementationscommit 848c78a84e5c7e11b2da953c9d9549b11fc5b1d8Author: abeck70 Date: Wed Mar 4 13:27:11 2015 +1100 OP-1696 reduce memory through re-use of pointers to data objects in base class.commit bc8794601142055942910ba9f938c03c8a3ca547Merge: 24bc3de 843325dAuthor: abeck70 Date: Tue Mar 3 22:06:01 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 24bc3de6155a74850b17ca7706f6b592923e027cAuthor: abeck70 Date: Tue Mar 3 17:56:12 2015 +1100 OP-1696 update gcs path planner options to align to new modescommit dc659891882ea0624a6436550425c54b347f4101Author: abeck70 Date: Tue Mar 3 13:51:55 2015 +1100 OP-1696 pathfollower class renaming and uncrustifycommit af19d114b8e0ff23159bdaca37e5f5bf1c1f8bebMerge: b07c1fd 04f4cd9Author: abeck70 Date: Mon Mar 2 21:47:12 2015 +1100 Merge branch rel-15.02 of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit b07c1fdd0544a555678bc7a8510cd92861f17e46Author: abeck70 Date: Mon Mar 2 21:27:27 2015 +1100 OP-1696 remainder of changes to integrate drive and fly objectivescommit c824c53cf30889a468ecb25ae3d705fa011b23baMerge: b89751c 1102243Author: abeck70 Date: Mon Mar 2 17:06:13 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit b89751caa5d6dc06e1e20391aa4e12b5484056d4Author: abeck70 Date: Mon Mar 2 17:04:27 2015 +1100 OP-1696 merge fly and drive pathdesired/action objectivescommit c463132cb7da5cdf9fc5395ae0650b081bc9fb55Author: abeck70 Date: Sun Mar 1 10:25:29 2015 +1100 OP-1696 merge ground pathfollower and refactored into C++ commit 6ed504b32f9e502e4f02d50b9e85ba91591417ba Merge: bcf245e deafed6 Author: abeck70 Date: Fri Feb 27 18:12:32 2015 +1100 Merge branch next into corvuscorax/OP-1642_tank-support Conflicts: flight/modules/PathFollower/pathfollower.c shared/uavobjectdefinition/vtolpathfollowersettings.xml commit bcf245e08c3348f2e6e5eddceee6d6cc268bdc23 Author: Corvus Corax Date: Sat Dec 6 19:45:29 2014 +0100 OP-1642 added new uavobject to discovery target too commit 6b9e8a7d7dad0739b4ffd2ce7f2f78ed4f597077 Author: Corvus Corax Date: Sat Dec 6 17:58:18 2014 +0100 OP-1642 added very simple path control for ground vehicles commit e8d8440e049c06158e7fc542627fac6420abaf65 Author: Corvus Corax Date: Sat Dec 6 17:17:09 2014 +0100 OP-1642 added new settings uavobject for tank path followercommit 71dd726eb5b52614b417236c22e8a5cc3a487fafAuthor: abeck70 Date: Fri Feb 27 18:05:10 2015 +1100 OP-1696 align pathaction to pathdesiredcommit 8a88af83be457aafbb63e4f94fad3b1cd54264caAuthor: abeck70 Date: Thu Feb 26 08:06:34 2015 +1100 OP-1696 fixed wing control refactored and compilingcommit fd9850b5e5680f5b422dbc468a94c5ec4b6dd518Author: abeck70 Date: Wed Feb 25 17:47:19 2015 +1100 OP-1696 Removed vtol stuff from pathfollower.cpp after having replaced it with class implcommit 9127d8d6d8c345c5bbf8622ce4e5eb76b03d1356Merge: 4054c73 93fb2d5Author: abeck70 Date: Wed Feb 25 13:09:06 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 4054c73ac98b54188a0a5367c5ca913b7fee6a89Author: abeck70 Date: Tue Feb 24 12:29:28 2015 +1100 OP-1696 fix compile errorscommit b398db3d4de8cd621a059da1f0dbb00868be0fb9Merge: 94c795a 3e15cacAuthor: abeck70 Date: Tue Feb 24 12:06:35 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 94c795af386cf0f1072a88e99a80cb1080bf85e7Merge: d2115de 57e0a8bAuthor: abeck70 Date: Mon Feb 23 17:54:33 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 Conflicts: flight/modules/Sensors/sensors.c make/common-defs.mkcommit d2115dee4e8e9b6a02f7b7700e4715480a56b18fAuthor: abeck70 Date: Mon Feb 23 14:35:02 2015 +1100 OP-1696 preliminary checkin for refactor of fly TODO: 1. Finalise and test fly brake 2. Remove patfhollower vtol fly 3. Rename all objects adding vtol in namecommit 95804b1d591d194cce3596187c74cdb2dd0d192fMerge: 58cebb0 31c1385Author: abeck70 Date: Sat Feb 21 22:10:03 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 58cebb0658deaece1555cfa24e50f1b86fd68c96Author: abeck70 Date: Sat Feb 21 22:09:29 2015 +1100 OP-1696 refactor neutral thrust and add pos hold implementationcommit 2c3cadfef3de9e092bd66bbabc58ac7847aca7e4Author: abeck70 Date: Fri Feb 20 18:32:11 2015 +1100 OP-1696 start of refactor of brakingcommit 991c2124e834f3432f5e34c5c9f4966cb42a7be3Merge: 1ab5798 db7d605Author: abeck70 Date: Thu Feb 19 12:17:54 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 1ab579865f04aeaa5899f5065a983de29074e025Author: abeck70 Date: Wed Feb 18 21:29:01 2015 +1100 OP-1696 Rewrote velocity roam into new C++ OO architecture With use of new PID controller for NE.commit 03edb049f06bf60054dce36a5b8b301a397284c4Author: abeck70 Date: Wed Feb 18 12:19:17 2015 +1100 OP-1696 Start of refactoring of VelocityRoam implementation in PathFollower Also TODOs: 1. To support pathplanner remove all references to FlightStatus object and add more fields to PathDesired 2. Refactor Braking to use C++ 3. Test and optimise Velocity Roam 4. Add more config options to Landing. For example disable disarming. And add more setting objects. 5. Consider moving all landing settings to one object.commit 1f5dba821d4535a9a14f333efdeec1af5afcc281Author: abeck70 Date: Mon Feb 16 22:51:31 2015 +1100 OP-1696 Landing tuningcommit 5133956c717fba871dd5700333806e298038a3d0Author: abeck70 Date: Mon Feb 16 21:05:06 2015 +1100 OP-1696 Landing Multiple fixescommit cf22055ecb8eb7304174c730f249a2332ebc599bAuthor: abeck70 Date: Sun Feb 15 20:52:29 2015 +1100 OP-1696 Add PIDStatus debug objectcommit bc13d732601e7c7cb48db17f1a75b33cc96c0c34Author: abeck70 Date: Sat Feb 14 15:53:16 2015 +1100 OP-1696 Landing sanity testedcommit 6e913076c1dfcb8b90153e9eca8acf6c9e51f293Author: abeck70 Date: Sat Feb 14 11:16:06 2015 +1100 OP-1696 completed refactoring 1. Completed refactoring of NE and Thrust control 2. Fixed velocity roam TODO Tuning and testingcommit 63b62fbd0ac105029e54163b98c3beebc2d73c3aAuthor: abeck70 Date: Fri Feb 13 08:54:23 2015 +1100 OP-1696 New PID Controller and refactoring of PID implementation for thrust managementcommit 7474331529477d0badd2ef0d609c84c556be9cecAuthor: abeck70 Date: Tue Feb 10 16:39:44 2015 +1100 OP-1696 Landing Refactor Thrust PID Loop for landingcommit 904ab2a9241e40c19da6a1508b4ce5243c75d7a3Author: abeck70 Date: Mon Feb 9 22:13:39 2015 +1100 OP-1696 Uncrustifycommit b3d30e298d8a0343782a29bc91be6677105b325eAuthor: abeck70 Date: Mon Feb 9 22:05:05 2015 +1100 OP-1696 Create new OO design Added PathFollowerControl which has an Landing implementation. The landing implementation runs its own set of PIDs. And interfaces to the FSM for Landing.commit 1ffd8d67cf7310c1c18c90f3ba18b08221d185a3Author: abeck70 Date: Sat Feb 7 18:04:42 2015 +1100 OP-1696 Upgrade arm toolscommit 3df01f97021d13694027b92ba0b13cc9fe70d437Author: abeck70 Date: Fri Feb 6 21:52:38 2015 +1100 OP-1696 Landing uncrustify and start of refactoring landing to make more use of FSMLandcommit 7a753b65fb69d52c89875b9a182ef5210ee8ef2dAuthor: abeck70 Date: Fri Feb 6 15:08:29 2015 +1100 OP-1696 Landing: fix compilation of all exescommit 692459874acf2b7ae07aa3ab88a2c78a09f26ee6Author: abeck70 Date: Fri Feb 6 14:45:06 2015 +1100 OP-1696 Landing startup fixescommit 9851f29b9238a969efd0110f01a4fad6bdbc5a5dAuthor: abeck70 Date: Fri Feb 6 10:21:52 2015 +1100 OP-1696 Landing pathfollower CPP refactoringcommit 9b3a54d41f14f07cd0d78426026d12ae9ac4e5b5Author: abeck70 Date: Wed Feb 4 13:02:44 2015 +1100 OP-1696 Moved to FSMLand class and fixed CPP problems 1. Additional methods compile options and need to have main as a cpp to solve various link issues when using static constructors but to avoid adding unnecessary libs.commit 7ebe739bfdb4e0f598f4408c1488f01bc033c044Author: abeck70 Date: Tue Feb 3 22:17:08 2015 +1100 OP-1696 Initial Landing prototype Proof of concept. To be refactored into C++commit 11d6754b9766efb76cc1cd57e9894badaf33ef2dAuthor: abeck70 Date: Tue Feb 3 22:13:10 2015 +1100 OP-1696 CPP Enablement revo c++ Enable optional revo C++ support: 1. Set USE_CXX to enable compliation and linkage of C++ source code 2. Disables rtti and exceptions 3. operator new and delete call pios malloc/free 4. Static constructor invocation supported #include #include #include static void SettingsUpdatedCb __attribute__ unused UAVObjEvent *ev static FrameType_t frameType static bool mode3D extern FrameType_t GetCurrentFrameType SettingsUpdatedCb NULL SystemSettingsConnectCallback &SettingsUpdatedCb VtolPathFollowerSettingsConnectCallback &SettingsUpdatedCb static void SettingsUpdatedCb __attribute__ unused UAVObjEvent *ev uint8_t TreatCustomCraftAs VtolPathFollowerSettingsTreatCustomCraftAsGet &TreatCustomCraftAs frameType = GetCurrentFrameType if frameType == FRAME_TYPE_CUSTOM switch TreatCustomCraftAs case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: frameType = FRAME_TYPE_FIXED_WING mode3D = true break case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: frameType = FRAME_TYPE_MULTIROTOR mode3D = true break case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: frameType = FRAME_TYPE_GROUND mode3D = false break cur &progress mode3D cur &progress mode3D
LibrePilot 14145 pathplanner.c Control e42c6e4265 Fixed PathPlanner s 3D mode for the case of non-custom crafts. switch frameType case FRAME_TYPE_GROUND: mode3D = false break default: mode3D = true
LibrePilot 14182 pathplanner.c Control e6de41855b OP-1740: GetSet use enums: Getting more files to conform to using enums instead of uint8_t... VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs
LibrePilot 14199 pathplanner.c Control 88b0c99087 OP-1760 autotakeoff in pathplaner: control to start location instead of end location in NE
LibrePilot 14200 pathplanner.c Control c4aa17a04c OP-1760 autotakeoff pathplanner now uses pathstatus in conditionLegRemaining PathStatusData pathStatus PathStatusGet &pathStatus if pathStatus.fractional_progress >= 1.0f - pathAction.ConditionParameters 0
LibrePilot 14204 pathplanner.c Non-Control d943f5a0ce OP-1760 autotakeoffOn landing stausvtolland state of DISARMED armhandler now detects this and disarms by force equivalent to a pathplanner alarm trigger.To re-arm one must leave the land flight mode.
LibrePilot 14229 pathplanner.c Control 210b15db4d OP-1858 autotakeoff in pathplanner1. Allowing arming in pathplanner2. Takeoff activation requires throttle to be above 30%3. Takeoff occurs at the current/actual position and rises vertical to the settings altititude.4. The details of the takeoff waypoint are effectively ignored - this is a simplicifcation to avoid having to have pathplanner inject a flyvector command from the current location to the waypoint5. We hard code the condition check for autotakeoff as only one mode is supported. #include #include static void planner_setup_pathdesired_takeoff PathDesiredData *pathDesired static void planner_setup_pathdesired PathDesiredData *pathDesired #define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f if !flightStatus.Armed return if pathAction.Command == PATHACTION_MODE_AUTOTAKEOFF pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING if uint8_t pathDesired.ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE ManualControlCommandData cmd ManualControlCommandGet &cmd if cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START pathDesired.ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE = float STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE PathDesiredSet &pathDesired return void updatePathDesired if !pathplanner_active return PathDesiredData pathDesired WaypointActiveGet &waypointActive WaypointInstGet waypointActive.Index &waypoint PathActionInstGet waypoint.Action &pathAction switch pathAction.Command case PATHACTION_MODE_AUTOTAKEOFF: planner_setup_pathdesired_takeoff &pathDesired break default: planner_setup_pathdesired &pathDesired break PathDesiredSet &pathDesired static void planner_setup_pathdesired PathDesiredData *pathDesired pathDesired->End.North = waypoint.Position.North pathDesired->End.East = waypoint.Position.East pathDesired->End.Down = waypoint.Position.Down pathDesired->EndingVelocity = waypoint.Velocity pathDesired->Mode = pathAction.Mode pathDesired->ModeParameters 0 = pathAction.ModeParameters 0 pathDesired->ModeParameters 1 = pathAction.ModeParameters 1 pathDesired->ModeParameters 2 = pathAction.ModeParameters 2 pathDesired->ModeParameters 3 = pathAction.ModeParameters 3 pathDesired->UID = waypointActive.Index pathDesired->Start.North = positionState.North pathDesired->Start.East = positionState.East pathDesired->Start.Down = positionState.Down pathDesired->StartingVelocity = pathDesired->EndingVelocity pathDesired->Start.North = waypointPrev.Position.North pathDesired->Start.East = waypointPrev.Position.East pathDesired->Start.Down = waypointPrev.Position.Down pathDesired->StartingVelocity = waypointPrev.Velocity #define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f #define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f static void planner_setup_pathdesired_takeoff PathDesiredData *pathDesired PositionStateData positionState PositionStateGet &positionState float velocity_down float autotakeoff_height FlightModeSettingsAutoTakeOffVelocityGet &velocity_down FlightModeSettingsAutoTakeOffHeightGet &autotakeoff_height autotakeoff_height = fabsf autotakeoff_height if autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN else if autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX pathDesired->Start.North = positionState.North pathDesired->Start.East = positionState.East pathDesired->Start.Down = positionState.Down pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH = 0.0f pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST = 0.0f pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN = -velocity_down pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE = float STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE pathDesired->End.North = positionState.North pathDesired->End.East = positionState.East pathDesired->End.Down = positionState.Down - autotakeoff_height pathDesired->StartingVelocity = 0.0f pathDesired->EndingVelocity = 0.0f pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF pathDesired->UID = waypointActive.Index
LibrePilot 14231 pathplanner.c Non-Control b14a966088 OP-1858 autotakeoff and pathplanner1. Allow arming in pathplanner2. Typo in comment
LibrePilot 14235 pathplanner.c Non-Control d6de7803e1 OP-1858 autotakeoff pathplanner fixes if pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF switch pathAction.Mode
LibrePilot 14240 pathplanner.c Control 0eb84cd8f6 OP-1867 pathplanner autolandSetup land with descent rate set from flight mode settings. #include static void planner_setup_pathdesired_land PathDesiredData *pathDesired StatusVtolAutoTakeoffInitialize StatusVtolLandInitialize case PATHACTION_MODE_LAND: planner_setup_pathdesired_land &pathDesired break static void planner_setup_pathdesired_land PathDesiredData *pathDesired PositionStateData positionState PositionStateGet &positionState float velocity_down FlightModeSettingsLandingVelocityGet &velocity_down pathDesired->Start.North = positionState.North pathDesired->Start.East = positionState.East pathDesired->Start.Down = positionState.Down pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH = 0.0f pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST = 0.0f pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN = velocity_down pathDesired->End.North = positionState.North pathDesired->End.East = positionState.East pathDesired->End.Down = positionState.Down pathDesired->StartingVelocity = 0.0f pathDesired->EndingVelocity = 0.0f pathDesired->Mode = PATHDESIRED_MODE_LAND pathDesired->ModeParameters PATHDESIRED_MODEPARAMETER_LAND_OPTIONS = float PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH
LibrePilot 14258 pathplanner.c Control 5ca9d979e6 OP-1858 if previous mode was autotakeoff ignore wp start position and go from current position. static void planner_setup_pathdesired PathDesiredData *pathDesired uint8_t uint8_t autotakeoff = pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF PathDesiredData pathDesired planner_setup_pathdesired &pathDesired autotakeoff static void planner_setup_pathdesired PathDesiredData *pathDesired uint8_t overwrite_start_position if waypointActive.Index == 0 || overwrite_start_position
LibrePilot 14267 pathplanner.c Non-Control 7fd08ef990 OP-1858 takeoff in nav address code review comments static void planner_setup_pathdesired PathDesiredData *pathDesired bool overwrite_start_position bool autotakeoff = pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF static void planner_setup_pathdesired PathDesiredData *pathDesired bool overwrite_start_position
LibrePilot 14494 pathplanner.c Control ccfe39633e OP-1900 Put Sequencing state machine for vixed wing autotakeoff out of plan.c and into vtolautotakeoffcontroller removed all special casing for autotakeoff and land from outside of pathfollower itself and code in plans.c - especially cleaned up pathplanner.c and made mode agnostic (which it should be) - added optional rewinding for pathplans pathplanner_active = true FlightModeSettingsFlightModeChangeRestartsPathPlanOptions restart FlightModeSettingsFlightModeChangeRestartsPathPlanGet &restart if restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE setWaypoint 0 return pathDesired.End.North = waypoint.Position.North pathDesired.End.East = waypoint.Position.East pathDesired.End.Down = waypoint.Position.Down pathDesired.EndingVelocity = waypoint.Velocity pathDesired.Mode = pathAction.Mode pathDesired.ModeParameters 0 = pathAction.ModeParameters 0 pathDesired.ModeParameters 1 = pathAction.ModeParameters 1 pathDesired.ModeParameters 2 = pathAction.ModeParameters 2 pathDesired.ModeParameters 3 = pathAction.ModeParameters 3 pathDesired.UID = waypointActive.Index if waypointActive.Index == 0 PositionStateData positionState PositionStateGet &positionState pathDesired.Start.North = positionState.North pathDesired.Start.East = positionState.East pathDesired.Start.Down = positionState.Down pathDesired.StartingVelocity = pathDesired.EndingVelocity else WaypointData waypointPrev WaypointInstGet waypointActive.Index - 1 &waypointPrev pathDesired.Start.North = waypointPrev.Position.North pathDesired.Start.East = waypointPrev.Position.East pathDesired.Start.Down = waypointPrev.Position.Down pathDesired.StartingVelocity = waypointPrev.Velocity
LibrePilot 14904 pathplanner.c Non-Control e1394febb0 LP-295 fixed file headers for all touched files * @addtogroup LibrePilotModules LibrePilot Modules * @author The LibrePilot Project http: * The OpenPilot Team http:
LibrePilot 15065 pathplanner.c Non-Control 134bcff0df LP-349 - Remove warning if pathplanner has no defined plan AlarmsClear SYSTEMALARMS_ALARM_PATHPLAN
LibrePilot 16097 pathplanner.c Control 9e788436d0 LP-601 Use normal altitude sign above home for AboveAltitude condition parameter * Parameter 0: altitude in meters if -positionState.Down >= pathAction.ConditionParameters 0
LibrePilot 16118 pathplanner.c Control 6364bb8566 LP-605 call setWaypoint() on PathPlan resume else setWaypoint waypointActive.Index return
LibrePilot 16119 pathplanner.c Non-Control 674129352d LP-605 uncrustify new code else setWaypoint waypointActive.Index
LibrePilot 984 attitude.c Non-Control 69de42db49 Add AHRS attitude module and driver skeletons - New Attitude module for AHRS (skeleton) - New AttitudeSettings UAVobject - New AttitudeActual UAVobject - Regenerated UAVobjects - Added new UAVobjects to OpenPilot and GCS builds - New PiOS driver for OpenPilot AHRS (stubs only)git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@655 ebee16cc-31ac-478f-84a7-5cbb03baadba #include attitude.h #include attitudeactual.h #include attitudesettings.h #include pios_opahrs.h #define STACK_SIZE 200 #define TASK_PRIORITY tskIDLE_PRIORITY+1 static xTaskHandle taskHandle static void attitudeTask void* parameters int32_t AttitudeInitialize void xTaskCreate attitudeTask signed char* Attitude STACK_SIZE NULL TASK_PRIORITY &taskHandle return 0 static void attitudeTask void* parameters AttitudeSettingsData settings AttitudeActualData data while 1 AttitudeSettingsGet &settings AttitudeActualGet &data PIOS_OPAHRS_ReadAttitude data.seq++ data.q1 += 0.111 data.q2 += 1.1 data.q3 += 7.0 data.q4 -= 2.321 data.ex += 0.01 data.ey -= 0.03 data.ez += 0.05 AttitudeActualSet &data vTaskDelay settings.UpdatePeriod / portTICK_RATE_MS
LibrePilot 990 attitude.c Non-Control 6b2760da1a USB HID: basic COM functionality over USB HIDThis fixes the COM interface over USB HID to the point whereit can establish and maintain solid communications with the UAVObjectBrowser in the GCS.Tested only on Linux. The USB HID interface is still disabled for nowuntil it is tested successfully by a wider group.Edit telemetry.c and set ALLOW_HID_TELEMETRY to 1 to enable telemetryover the USB HID interface and report your results in the forum.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@656 ebee16cc-31ac-478f-84a7-5cbb03baadba #define TASK_PRIORITY tskIDLE_PRIORITY+2
LibrePilot 998 attitude.c Non-Control 9be49ca54d Flight/Attitude Minor change in field names and task prioritygit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@662 ebee16cc-31ac-478f-84a7-5cbb03baadba #define TASK_PRIORITY tskIDLE_PRIORITY+4 data.Roll += 0.01 data.Pitch -= 0.03 data.Yaw += 0.05
LibrePilot 1119 attitude.c Non-Control e2176d3117 - renamed PiOS.x86 into PiOS.posix- created temporary branch of OpenPilot (OpenPilot.posix) in order to test multi platform changes on hardware before committing to the main branchgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@995 ebee16cc-31ac-478f-84a7-5cbb03baadba #include attitude.h #include attitudeactual.h #include attitudesettings.h #include pios_opahrs.h #define STACK_SIZE 200 #define TASK_PRIORITY tskIDLE_PRIORITY+4 static xTaskHandle taskHandle static void attitudeTask void* parameters int32_t AttitudeInitialize void xTaskCreate attitudeTask signed char* Attitude STACK_SIZE NULL TASK_PRIORITY &taskHandle return 0 static void attitudeTask void* parameters AttitudeSettingsData settings AttitudeActualData data while 1 AttitudeSettingsGet &settings AttitudeActualGet &data PIOS_OPAHRS_ReadAttitude data.seq++ data.q1 += 0.111 data.q2 += 1.1 data.q3 += 7.0 data.q4 -= 2.321 data.Roll += 0.01 data.Pitch -= 0.03 data.Yaw += 0.05 AttitudeActualSet &data vTaskDelay settings.UpdatePeriod / portTICK_RATE_MS
LibrePilot 1184 attitude.c Non-Control 1cc6981ee5 attitude: Remove seq field from uavobjectThe sequence number field for the attitude solution islikely unnecessary. Removed.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1000 ebee16cc-31ac-478f-84a7-5cbb03baadba
LibrePilot 1210 attitude.c Non-Control c7479b796b merged chages 995:1015 into posix branch(See? That s why I hate branching - thank the gods that subversion has the merge command)git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1016 ebee16cc-31ac-478f-84a7-5cbb03baadba
LibrePilot 2932 attitude.c Non-Control 1cf7555b7d CC-10 Rename CCAttitude to Attitude. If there are alternative ones we willjust have nested subdirectories of Attitudegit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2655 ebee16cc-31ac-478f-84a7-5cbb03baadba * @addtogroup Attitude Copter Control Attitude Estimation * @brief Acquires sensor data and computes attitude estimate * @file attitude.c #include attitude.h static void AttitudeTask void *parameters int32_t AttitudeInitialize void xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &taskHandle static void AttitudeTask void *parameters
LibrePilot 2944 attitude.c Control 3f088bed8d CC-9 1. Improve the timings of the attitude estimation2. Disable FirmareIAP module in CC (somehow causes lockups when also using vTaskDelayUntil in Attitude WTF)3. Make the SPI bus run a little faster so we can handle the 3200 Hz from accelwhile running the filter at 333 Hzgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2664 ebee16cc-31ac-478f-84a7-5cbb03baadba #define STACK_SIZE_BYTES 440 #define TASK_PRIORITY tskIDLE_PRIORITY+3 #define UPDATE_RATE 3 static portTickType lastSysTime vTaskDelayUntil &lastSysTime UPDATE_RATE / portTICK_RATE_MS #define UPDATE_FRAC 0.999f
LibrePilot 2946 attitude.c Control 1c6b51b704 UAVObjects/AttitudeSettinsg: Object for CC to tune the attitude estimationgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2666 ebee16cc-31ac-478f-84a7-5cbb03baadba #include attitudesettings.h AttitudeSettingsData settings AttitudeSettingsGet &settings float tau = 1-settings.GyroBiasTau #define UPDATE_FRAC 0.99999f AttitudeSettingsData settings AttitudeSettingsGet &settings float tau = 1-settings.AccelTau attitudeActual.Roll = tau * attitudeActual.Roll + 1-tau * accel_roll * 180 / M_PI attitudeActual.Pitch = tau * attitudeActual.Pitch + 1-tau * accel_pitch * 180 / M_PI
LibrePilot 2947 attitude.c Control b1e4b65f3d CC-1 Bigger oversampling window for CC sensor datagit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2667 ebee16cc-31ac-478f-84a7-5cbb03baadba PIOS_ADC_Config PIOS_ADC_RATE / 1000 / UPDATE_RATE
LibrePilot 2966 attitude.c Control 1663a838ff CC-7 Full complimentary filter ala Mahoney paper using quaternionrepresentation. Also improved gyro bias initialization.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2691 ebee16cc-31ac-478f-84a7-5cbb03baadba #define STACK_SIZE_BYTES 540 static float gyro_correct_int 3 = 0 0 0 static void initSensors static void updateSensors static void updateAttitude AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude initSensors static void initSensors vTaskDelay 50 updateSensors AttitudeRawData attitudeRaw AttitudeRawGet &attitudeRaw AttitudeSettingsData settings AttitudeSettingsGet &settings gyro_correct_int 0 = - attitudeRaw.gyros_filtered 0 gyro_correct_int 1 = - attitudeRaw.gyros_filtered 1 gyro_correct_int 2 = - attitudeRaw.gyros_filtered 2 static void updateSensors attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_X += gyro_correct_int 0 attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Y += gyro_correct_int 1 gyro_correct_int 2 = 1-settings.GyroBiasTau * gyro_correct_int 2 - settings.GyroBiasTau * attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Z attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Z += gyro_correct_int 2 static void updateAttitude float * q = &attitudeActual.q1 float gyro 3 = attitudeRaw.gyros_filtered 0 attitudeRaw.gyros_filtered 1 attitudeRaw.gyros_filtered 2 float * accels = attitudeRaw.accels_filtered float grot 3 float accel_err 3 grot 0 = -9.81 * 2 * q 1 * q 3 - q 0 * q 2 grot 1 = -9.81 * 2 * q 2 * q 3 + q 0 * q 1 grot 2 = -9.81 * q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 CrossProduct const float * accels const float * grot accel_err gyro_correct_int 0 += accel_err 0 * settings.AccelKI * dT gyro_correct_int 1 += accel_err 1 * settings.AccelKI * dT gyro 0 += accel_err 0 * settings.AccelKp gyro 1 += accel_err 1 * settings.AccelKp gyro 2 += accel_err 2 * settings.AccelKp float qdot 4 qdot 0 = -q 1 * gyro 0 - q 2 * gyro 1 - q 3 * gyro 2 * dT * M_PI / 180 / 2 qdot 1 = q 0 * gyro 0 - q 3 * gyro 1 + q 2 * gyro 2 * dT * M_PI / 180 / 2 qdot 2 = q 3 * gyro 0 + q 0 * gyro 1 - q 1 * gyro 2 * dT * M_PI / 180 / 2 qdot 3 = -q 2 * gyro 0 + q 1 * gyro 1 + q 0 * gyro 2 * dT * M_PI / 180 / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 float qmag = sqrt q 0 *q 0 + q 1 *q 1 + q 2 *q 2 + q 3 *q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag attitudeActual.q1 = q 0 attitudeActual.q2 = q 1 attitudeActual.q3 = q 2 attitudeActual.q4 = q 3 Quaternion2RPY q &attitudeActual.Roll
LibrePilot 2967 attitude.c Control 265737427b CC-7: Make the gyro scale adjustable (for now at least this was largely fordebugging let s compare values).git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2692 ebee16cc-31ac-478f-84a7-5cbb03baadba attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_X = - gyro 0 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Y = gyro 1 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Z = - gyro 2 - GYRO_NEUTRAL * settings.GyroGain gyro_correct_int 2 += - attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Z * settings.AccelKI * UPDATE_RATE / 1000
LibrePilot 2968 attitude.c Control 986e124202 CC-7 Reverse order of two lines so that yaw bias converges instead of diverges:-)git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2693 ebee16cc-31ac-478f-84a7-5cbb03baadba gyro_correct_int 2 += - attitudeRaw.gyros_filtered ATTITUDERAW_GYROS_FILTERED_Z * settings.AccelKI * UPDATE_RATE / 1000
LibrePilot 2976 attitude.c Control c95a6447bf CC-8 Whenever the gyro data is updated the callback now pushes the data onto aqueue which update sensors waits for. This confirms one update per gyroupdate. All available accel data will be pulled off each time.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2705 ebee16cc-31ac-478f-84a7-5cbb03baadba static xQueueHandle gyro_queue gyro_queue = xQueueCreate 2 sizeof float * 3 if gyro_queue == NULL return -1 float gyro 3 if xQueueReceive gyro_queue void * const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return static portBASE_TYPE xHigherPriorityTaskWoken xQueueSendFromISR gyro_queue data &xHigherPriorityTaskWoken portEND_SWITCHING_ISR xHigherPriorityTaskWoken
LibrePilot 2978 attitude.c Control 810fa70856 UAVObjects: Remove the raw versus filtered fields in the sensor data. Savesome memory.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2706 ebee16cc-31ac-478f-84a7-5cbb03baadba gyro_correct_int 0 = - attitudeRaw.gyros 0 gyro_correct_int 1 = - attitudeRaw.gyros 1 gyro_correct_int 2 = - attitudeRaw.gyros 2 attitudeRaw.gyros ATTITUDERAW_GYROS_X = - gyro 0 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros ATTITUDERAW_GYROS_Y = gyro 1 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros ATTITUDERAW_GYROS_Z = - gyro 2 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros ATTITUDERAW_GYROS_X += gyro_correct_int 0 attitudeRaw.gyros ATTITUDERAW_GYROS_Y += gyro_correct_int 1 attitudeRaw.gyros ATTITUDERAW_GYROS_Z += gyro_correct_int 2 gyro_correct_int 2 += - attitudeRaw.gyros ATTITUDERAW_GYROS_Z * attitudeRaw.accels ATTITUDERAW_ACCELS_X = 0 attitudeRaw.accels ATTITUDERAW_ACCELS_Y = 0 attitudeRaw.accels ATTITUDERAW_ACCELS_Z = 0 attitudeRaw.accels ATTITUDERAW_ACCELS_X += float accel_data.x * 0.004f * 9.81 attitudeRaw.accels ATTITUDERAW_ACCELS_Y += - float accel_data.y * 0.004f * 9.81 attitudeRaw.accels ATTITUDERAW_ACCELS_Z += - float accel_data.z * 0.004f * 9.81 attitudeRaw.accels ATTITUDERAW_ACCELS_X /= i attitudeRaw.accels ATTITUDERAW_ACCELS_Y /= i attitudeRaw.accels ATTITUDERAW_ACCELS_Z /= i float * gyro = attitudeRaw.gyros float * accels = attitudeRaw.accels
LibrePilot 2981 attitude.c Non-Control fedadb1275 CC-8: Move the queue registration into the ADC PIOS driver to allow otherfunctions to use it easilyConflicts:flight/Modules/Attitude/attitude.cgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2707 ebee16cc-31ac-478f-84a7-5cbb03baadba gyro_queue = xQueueCreate 2 sizeof float * 4 PIOS_ADC_SetQueue gyro_queue float gyro 4 attitudeRaw.gyros ATTITUDERAW_GYROS_X = - gyro 1 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros ATTITUDERAW_GYROS_Y = gyro 2 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw.gyros ATTITUDERAW_GYROS_Z = - gyro 3 - GYRO_NEUTRAL * settings.GyroGain
LibrePilot 2983 attitude.c Non-Control e5b26bc67d More memory tweaks to keep CC flowinggit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2709 ebee16cc-31ac-478f-84a7-5cbb03baadba gyro_queue = xQueueCreate 1 sizeof float * 4
LibrePilot 3021 attitude.c Control 03fb82ad0a CC Attitude: Change when AttitudeRaw is set to end of attitude estimation because this is what is determines when the Stabilization code executes.Because we aren t oversampling gyros relative to the PID in CC we should setthe attitude first (computational time to do so is negligible).Also change update rate to 500 Hz.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2750 ebee16cc-31ac-478f-84a7-5cbb03baadba #define UPDATE_RATE 2.0f static void updateSensors AttitudeRawData * static void updateAttitude AttitudeRawData * PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE AttitudeRawData attitudeRaw AttitudeRawGet &attitudeRaw updateSensors &attitudeRaw updateAttitude &attitudeRaw AttitudeRawSet &attitudeRaw updateSensors &attitudeRaw static void updateSensors AttitudeRawData * attitudeRaw attitudeRaw->gyros ATTITUDERAW_GYROS_X = - gyro 1 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw->gyros ATTITUDERAW_GYROS_Y = gyro 2 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw->gyros ATTITUDERAW_GYROS_Z = - gyro 3 - GYRO_NEUTRAL * settings.GyroGain attitudeRaw->gyros ATTITUDERAW_GYROS_X += gyro_correct_int 0 attitudeRaw->gyros ATTITUDERAW_GYROS_Y += gyro_correct_int 1 attitudeRaw->gyros ATTITUDERAW_GYROS_Z += gyro_correct_int 2 gyro_correct_int 2 += - attitudeRaw->gyros ATTITUDERAW_GYROS_Z * attitudeRaw->accels ATTITUDERAW_ACCELS_X = 0 attitudeRaw->accels ATTITUDERAW_ACCELS_Y = 0 attitudeRaw->accels ATTITUDERAW_ACCELS_Z = 0 attitudeRaw->gyrotemp 0 = PIOS_ADXL345_Read &accel_data attitudeRaw->accels ATTITUDERAW_ACCELS_X += float accel_data.x * 0.004f * 9.81 attitudeRaw->accels ATTITUDERAW_ACCELS_Y += - float accel_data.y * 0.004f * 9.81 attitudeRaw->accels ATTITUDERAW_ACCELS_Z += - float accel_data.z * 0.004f * 9.81 while i < 32 && attitudeRaw->gyrotemp 0 > 0 attitudeRaw->gyrotemp 1 = i attitudeRaw->accels ATTITUDERAW_ACCELS_X /= i attitudeRaw->accels ATTITUDERAW_ACCELS_Y /= i attitudeRaw->accels ATTITUDERAW_ACCELS_Z /= i static void updateAttitude AttitudeRawData * attitudeRaw float * gyro = attitudeRaw->gyros float * accels = attitudeRaw->accels
LibrePilot 3023 attitude.c Control 74cc7dbfc9 CC-20 Attitude: Jack up Kp and Ki values for first 5 seconds to force fasterconvergence of gyro bias then the normal values can be lower.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2753 ebee16cc-31ac-478f-84a7-5cbb03baadba static void settingsUpdatedCb UAVObjEvent * objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42 AttitudeSettingsConnectCallback &settingsUpdatedCb uint8_t init = 0 if xTaskGetTickCount < 5000 accelKi = 10 accelKp = 1 else if init == 0 settingsUpdatedCb AttitudeSettingsHandle init = 1 attitudeRaw->gyros ATTITUDERAW_GYROS_X = - gyro 1 - GYRO_NEUTRAL * gyroGain attitudeRaw->gyros ATTITUDERAW_GYROS_Y = gyro 2 - GYRO_NEUTRAL * gyroGain attitudeRaw->gyros ATTITUDERAW_GYROS_Z = - gyro 3 - GYRO_NEUTRAL * gyroGain accelKi * UPDATE_RATE / 1000 gyro_correct_int 0 += accel_err 0 * accelKi * dT gyro_correct_int 1 += accel_err 1 * accelKi * dT gyro 0 += accel_err 0 * accelKp gyro 1 += accel_err 1 * accelKp gyro 2 += accel_err 2 * accelKp static void settingsUpdatedCb UAVObjEvent * objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKI gyroGain = attitudeSettings.GyroGain
LibrePilot 3024 attitude.c Control cfe295377c CC-18: Normalize the gravity vector to unity gaingit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2754 ebee16cc-31ac-478f-84a7-5cbb03baadba if xTaskGetTickCount < 10000 accelKp = 1000 accelKi = 100 / xTaskGetTickCount / 2000 float gyro 3 gyro 0 = attitudeRaw->gyros 0 gyro 1 = attitudeRaw->gyros 1 gyro 2 = attitudeRaw->gyros 2 grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 float accel_mag = sqrt accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag accelKi = attitudeSettings.AccelKi
LibrePilot 3025 attitude.c Control 9f71f4121c CC-18 Attitude: Change units to proper engineering ones. An accelKp=1 meansthat it will completely follow the accel attitude each cycle and is way toohigh. Ki=1 means the gyro bias wil be correct by accels each cycle (way toohigh).git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2755 ebee16cc-31ac-478f-84a7-5cbb03baadba accelKp = 1 accelKi = .1 / xTaskGetTickCount / 5000 accelKi gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyro 0 += accel_err 0 * accelKp / dT gyro 1 += accel_err 1 * accelKp / dT gyro 2 += accel_err 2 * accelKp / dT
LibrePilot 3029 attitude.c Control 8498512855 CC-20 Attitude initialization: Make sure no divide by 0 condition possiblegit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2761 ebee16cc-31ac-478f-84a7-5cbb03baadba accelKi = .5 / 1 + xTaskGetTickCount / 5000
LibrePilot 3098 attitude.c Control 9a2bdac49c Reduced CC CPU usage from 57% to 46% by moving the floating point multiplies outside of the accelerometer downsample/averaging loop in the updateSensors() function.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2809 ebee16cc-31ac-478f-84a7-5cbb03baadba int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 attitudeRaw->gyrotemp 0 = samples_remaining attitudeRaw->gyrotemp 1 = i attitudeRaw->accels ATTITUDERAW_ACCELS_X = float x * 0.004f * 9.81f / i attitudeRaw->accels ATTITUDERAW_ACCELS_Y = float y * 0.004f * 9.81f / i attitudeRaw->accels ATTITUDERAW_ACCELS_Z = float z * 0.004f * 9.81f / i
LibrePilot 3140 attitude.c Control ea390d0b90 CC-26: Add accel bias term. Note it s units are LSB not m/s^sgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2895 ebee16cc-31ac-478f-84a7-5cbb03baadba static int16_t accelbias 3 x -= accelbias 0 y -= accelbias 1 z -= accelbias 2 accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z
LibrePilot 3177 attitude.c Non-Control e9d507dd3e CC: Fixed compilation error after uavobject renamegit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2966 ebee16cc-31ac-478f-84a7-5cbb03baadba #include stabilizationdesired.h
LibrePilot 3178 attitude.c Non-Control 8d085552fb CC: Modules/Attitude/attitude.c: unused headers removed (do we have another?)git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2967 ebee16cc-31ac-478f-84a7-5cbb03baadba
LibrePilot 3222 attitude.c Control 2e5863281c CC/Stabilization - Fix attitude bias not being applied over all sampled quantitiesgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3065 ebee16cc-31ac-478f-84a7-5cbb03baadba x -= accelbias 0 * i y -= accelbias 1 * i z -= accelbias 2 * i
LibrePilot 3230 attitude.c Control bd5925d526 CC-35: Separate term for CC yaw bias rate and othersNote: AttitudeSettings changed so you ll need to re-zero itgit-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3090 ebee16cc-31ac-478f-84a7-5cbb03baadba static float yawBiasRate = 0 yawBiasRate = 0.01 / 1 + xTaskGetTickCount / 5000 yawBiasRate yawBiasRate = attitudeSettings.YawBiasRate
LibrePilot 3232 attitude.c Control a57c289b3a CC-24: Support mounting CC at any angle by applying a rotation. Now need torotate sensors to make sure stabilization behaves well so don t use till then.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3100 ebee16cc-31ac-478f-84a7-5cbb03baadba static float rotationQuat 4 = 1 0 0 0 static float q 4 = 1 0 0 0 static int8_t rotate = 0 if rotate AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual if rotate quat_mult q rotationQuat &attitudeActual.q1 else quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll if attitudeSettings.RotationQuaternion 0 == 126 rotate = 0 rotationQuat 0 = 1 rotationQuat 1 = rotationQuat 2 = rotationQuat 3 = 0 return rotate = 1 rotationQuat 0 = float attitudeSettings.RotationQuaternion 0 / 127.0 rotationQuat 1 = float attitudeSettings.RotationQuaternion 1 / 127.0f rotationQuat 2 = float attitudeSettings.RotationQuaternion 2 / 127.0f rotationQuat 3 = float attitudeSettings.RotationQuaternion 3 / 127.0f float len = sqrt rotationQuat 0 * rotationQuat 0 + rotationQuat 1 * rotationQuat 1 + rotationQuat 2 * rotationQuat 2 + rotationQuat 3 * rotationQuat 3 if len < 1e-3 rotationQuat 0 = 1 rotationQuat 1 = rotationQuat 2 = rotationQuat 3 = 0 rotate = 0 else rotationQuat 0 /= len rotationQuat 1 /= len rotationQuat 2 /= len rotationQuat 3 /= len
LibrePilot 3251 attitude.c Control 3afdc7e41c CC-24: Rotate the CC board at any angle relative to flat and level with GCSconfig plugin updates. Has not been tested in flight yet although seemssensible so please be careful when using this code.git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3166 ebee16cc-31ac-478f-84a7-5cbb03baadba static float R 3 3 float accel 3 = float x / i float y / i float z / i float vec_out 3 rot_mult R accel vec_out attitudeRaw->accels 0 = vec_out 0 attitudeRaw->accels 1 = vec_out 1 attitudeRaw->accels 2 = vec_out 2 rot_mult R attitudeRaw->gyros vec_out attitudeRaw->gyros 0 = vec_out 0 attitudeRaw->gyros 1 = vec_out 1 attitudeRaw->gyros 2 = vec_out 2 else attitudeRaw->accels 0 = accel 0 attitudeRaw->accels 1 = accel 1 attitudeRaw->accels 2 = accel 2 attitudeRaw->accels ATTITUDERAW_ACCELS_X = attitudeRaw->accels ATTITUDERAW_ACCELS_X - accelbias 0 * 0.004f * 9.81f attitudeRaw->accels ATTITUDERAW_ACCELS_Y = attitudeRaw->accels ATTITUDERAW_ACCELS_Y - accelbias 1 * 0.004f * 9.81f attitudeRaw->accels ATTITUDERAW_ACCELS_Z = attitudeRaw->accels ATTITUDERAW_ACCELS_Z - accelbias 2 * 0.004f * 9.81f attitudeRaw->gyros ATTITUDERAW_GYROS_X += gyro_correct_int 0 attitudeRaw->gyros ATTITUDERAW_GYROS_Y += gyro_correct_int 1 attitudeRaw->gyros ATTITUDERAW_GYROS_Z += gyro_correct_int 2 gyro_correct_int 2 += - attitudeRaw->gyros ATTITUDERAW_GYROS_Z * yawBiasRate quat_copy q &attitudeActual.q1 float rotationQuat 4 Quaternion2R rotationQuat R
LibrePilot 3334 attitude.c Control 7e418866be Update the AttitudeSetting object to make the board rotation more humanreadable and update the GCS fields appropriately. if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1
LibrePilot 3345 attitude.c Control 4ec0263dfc OP-476: If AttitudeSettings.ZeroDuringArming is true then while arming (1second) it will speed up the estimate of gyro bias. #include flightstatus.h static bool zero_during_arming = false zero_during_arming = false FlightStatusData flightStatus FlightStatusGet &flightStatus init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKi = .01 yawBiasRate = 0.1 init = 0 zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE
LibrePilot 3357 attitude.c Control dbf8a77a94 Now there are other attitude settings (like rotation angle) make sure these areloaded right away at power up settingsUpdatedCb AttitudeSettingsHandle
LibrePilot 3365 attitude.c Control 84e09031a7 Faster attitude calc be much more aggressive with working out bias.Change AccelkP to 0.05 to give accels more influence just balacing out the 3C filter.Change stab setting to be a better default fopr most Quads. if xTaskGetTickCount < 7000 accelKi = 0.9 yawBiasRate = 0.23 yawBiasRate = 0.23
LibrePilot 3370 attitude.c Control 71d582495e On arm bias calibration terms now matches boot calibration terms accelKi = 0.9
LibrePilot 3372 attitude.c Control 5f32803763 Changed accelKp = 1; in 3C filter when calibrating bias when arming. Increase default yaw by 0.005 accelKp = 1
LibrePilot 3488 attitude.c Non-Control 65cf467ca4 OP-423: move the module initialize funtion into a specific section for OP and CC.- create linker section for those Initialize()- later this list will incorporate parameters as well. (this probably will be more a OP feature to swap/remove/delete module on the fly.- this is not done at compile time anymore by Makefile.- this will allow us to have control on the module start at run-time (not implemented but build the ground for it).- this simplify the startup (Part of code re-org).- this change does not affect sim_posix and win32 (since they don t need that)- ensure it s compiling for PiOS.posix- port to PiOS.win32 but not tested (not compiled)- tested on CC- compile on OP.- this free ~200 bytes.- current avalable bytes (is we keep the same remaining bytes on the stack than before) is easily passed the 1.2Ko mark on CC with new gcc (4.5.2)- this does not include init-reorg for each module (I still think more can be freed) module_initcall AttitudeInitialize 0
LibrePilot 3507 attitude.c Control 5044ea36de Start initializing objects in the modules that consume them. Shouldn t affectOP yet but not tested. AttitudeActualInitialize AttitudeRawInitialize AttitudeSettingsInitialize
LibrePilot 3528 attitude.c Control fc1e3f574c OP-423: Split task create and module init in order to postpone task creation once the full heap is available.Also implement some ordering (quite ugly still) in the module init and task creation order so we can decide which module to start/init firstand which module to start/init last.This will be replaced/adapter with the uavobject list later (once it s implemented).reserving some space for module init and task create parameters to customize module/task creation (this will be usefull once we get the list and customization from customer).Changes have been made for OP and CC. Tested comped with CC OP sim_posix.Only ran on bench with CC for couple of minutes (code increase expected but no dropping of stack which is good).This gives task creation at the time wherethe all heap is available. int32_t AttitudeStart void xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &taskHandle TaskMonitorAdd TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeSettingsConnectCallback &settingsUpdatedCb if gyro_queue == NULL module_initcall AttitudeInitialize 0 AttitudeStart 0 MODULE_EXEC_NOORDER_FLAG
LibrePilot 3554 attitude.c Control 0a4bbcc12e Added a field for initial gyro bias to speed up initialization. * @brief Acquires sensor data and computes attitude estimate static bool bias_correct_gyro = true if gyro_queue == NULL PIOS_FLASH_DISABLE init = 0 AttitudeRawGet &attitudeRaw updateSensors &attitudeRaw AttitudeRawSet &attitudeRaw static void updateSensors AttitudeRawData * attitudeRaw if bias_correct_gyro attitudeRaw->gyros ATTITUDERAW_GYROS_X += gyro_correct_int 0 attitudeRaw->gyros ATTITUDERAW_GYROS_Y += gyro_correct_int 1 attitudeRaw->gyros ATTITUDERAW_GYROS_Z += gyro_correct_int 2 gyro_correct_int 2 += - attitudeRaw->gyros ATTITUDERAW_GYROS_Z * yawBiasRate Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll accelKi = attitudeSettings.AccelKi bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH
LibrePilot 3556 attitude.c Non-Control ab7ff56d96 Compute initial gyro bias while calibrating to speed up convergence
LibrePilot 3558 attitude.c Control e6ad21d881 Make gyro bias in deg/s * 100 to calibrate more precisely. gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f
LibrePilot 3563 attitude.c Non-Control de55c56427 OP-423: Change capital on macro to be uppercase for consistency. MODULE_INITCALL AttitudeInitialize 0 AttitudeStart 0 MODULE_EXEC_NOORDER_FLAG
LibrePilot 3575 attitude.c Control 4c0245281d Now we store the gyro bias avoid quashing the initialization correction. AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate
LibrePilot 3617 attitude.c Non-Control 9b9b5a1367 OP-423: remove ; at the end of macro MODULE_INITCALL AttitudeInitialize 0 AttitudeStart 0 MODULE_EXEC_NOORDER_FLAG
LibrePilot 3628 attitude.c Control 2d995dc2f7 OP-539: Don t call the gyro setting callback during init because it clears thegyro bias estimation. settingsUpdatedCb AttitudeSettingsHandle if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000
LibrePilot 3647 attitude.c Control 2c7cfe435f Somehow this task runs quite a few times at the beginning so dT was going tozero and making the attitude get NaN. Wrote recovery code for that condition(should never occur) and also force minimum dT to 1 ms (also shouldn t occur) gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 AttitudeSettingsConnectCallback &settingsUpdatedCb float dT portTickType thisSysTime = xTaskGetTickCount dT = thisSysTime == lastSysTime ? 0.001 : portMAX_DELAY & thisSysTime - lastSysTime / portTICK_RATE_MS / 1000.0f if fabs qmag < 1e-3 || qmag != qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0
LibrePilot 3652 attitude.c Non-Control 612a439199 OP-423: simplify the MODULE_INITCALL macro and remove the ordering loops MODULE_INITCALL AttitudeInitialize AttitudeStart
LibrePilot 3836 attitude.c Control ec2121bca8 Attitude: Keep first element of quaternion positive if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3
LibrePilot 3888 attitude.c Control a2b76adc33 Attitude: Do not start attitude estimation until accel data appears. Thisfixes the NaN when AttitudeSettings not available. static int8_t updateSensors AttitudeRawData * while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE if updateSensors &attitudeRaw != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else updateAttitude &attitudeRaw AttitudeRawSet &attitudeRaw AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int8_t updateSensors AttitudeRawData * attitudeRaw return -1 if PIOS_ADXL345_FifoElements == 0 return -1 return 0
LibrePilot 4098 attitude.c Non-Control f725fbe128 CopterControl: Fix build due to changing the AttitudeRaw structure. attitudeRaw->temperature ATTITUDERAW_TEMPERATURE_GYRO = samples_remaining attitudeRaw->temperature ATTITUDERAW_TEMPERATURE_ACCEL = i
LibrePilot 4440 attitude.c Control 2cc2e55248 Add attitude solution for revo #include pios.h #include attitude.h #include attituderaw.h #include attitudeactual.h #include attitudesettings.h #include baroaltitude.h #include flightstatus.h #include CoordinateConversions.h #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY tskIDLE_PRIORITY+3 #define PI_MOD x fmod x + M_PI M_PI * 2 - M_PI static xTaskHandle taskHandle static void AttitudeTask void *parameters static float gyro_correct_int 3 = 0 0 0 static int8_t updateSensors AttitudeRawData * static void updateAttitude AttitudeRawData * static void settingsUpdatedCb UAVObjEvent * objEv static float accelKi = 0 static float accelKp = 0 static float yawBiasRate = 0 static float gyroGain = 0.42 static int16_t accelbias 3 static float q 4 = 1 0 0 0 static float R 3 3 static int8_t rotate = 0 static bool zero_during_arming = false static bool bias_correct_gyro = true int32_t AttitudeStart void xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &taskHandle TaskMonitorAdd TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 int32_t AttitudeInitialize void AttitudeActualInitialize AttitudeRawInitialize AttitudeSettingsInitialize BaroAltitudeInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 MODULE_INITCALL AttitudeInitialize AttitudeStart int32_t accel_test int32_t gyro_test int32_t mag_test int32_t pressure_test static void AttitudeTask void *parameters uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb AttitudeSettingsHandle accel_test = PIOS_BMA180_Test gyro_test = PIOS_MPU6050_Test mag_test = PIOS_HMC5883_Test pressure_test = PIOS_BMP085_Test PIOS_BMP085_StartADC TemperatureConv while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1 accelKi = 0.9 yawBiasRate = 0.23 init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1 accelKi = 0.9 yawBiasRate = 0.23 init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AttitudeRawData attitudeRaw AttitudeRawGet &attitudeRaw if updateSensors &attitudeRaw != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AttitudeRawSet &attitudeRaw AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE vTaskDelay 1 uint32_t accel_samples uint32_t gyro_samples struct pios_bma180_data accel struct pios_mpu6050_data gyro AttitudeRawData raw int32_t accel_accum 3 = 0 0 0 int32_t gyro_accum 3 = 0 0 0 float scaling static int8_t updateSensors AttitudeRawData * attitudeRaw int32_t read_good int32_t count for int i = 0 i < 3 i++ accel_accum i = 0 gyro_accum i = 0 accel_samples = 0 gyro_samples = 0 count = 0 while read_good = PIOS_BMA180_ReadFifo &accel != 0 while read_good == 0 count++ accel_accum 0 += accel.x accel_accum 1 += accel.y accel_accum 2 += accel.z read_good = PIOS_BMA180_ReadFifo &accel accel_samples = count scaling = PIOS_BMA180_GetScale / accel_samples attitudeRaw->accels ATTITUDERAW_ACCELS_X = accel_accum 0 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Y = accel_accum 1 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Z = accel_accum 2 * scaling attitudeRaw->temperature ATTITUDERAW_TEMPERATURE_ACCEL = 25.0f + float accel.temperature - 2 / 2 if bias_correct_gyro attitudeRaw->gyros ATTITUDERAW_GYROS_X += gyro_correct_int 0 attitudeRaw->gyros ATTITUDERAW_GYROS_Y += gyro_correct_int 1 attitudeRaw->gyros ATTITUDERAW_GYROS_Z += gyro_correct_int 2 gyro_correct_int 2 += - attitudeRaw->gyros ATTITUDERAW_GYROS_Z * yawBiasRate if PIOS_HMC5883_NewDataAvailable int16_t values 3 PIOS_HMC5883_ReadMag values attitudeRaw->magnetometers ATTITUDERAW_MAGNETOMETERS_X = -values 0 attitudeRaw->magnetometers ATTITUDERAW_MAGNETOMETERS_Y = -values 1 attitudeRaw->magnetometers ATTITUDERAW_MAGNETOMETERS_Z = -values 2 AttitudeRawSet &raw int32_t retval = PIOS_BMP085_ReadADC if retval == 0 static uint32_t baro_conversions if baro_conversions++ % 2 PIOS_BMP085_StartADC PressureConv else PIOS_BMP085_StartADC TemperatureConv float pressure pressure = PIOS_BMP085_GetPressure BaroAltitudeData data BaroAltitudeGet &data data.Altitude = 1.0f - powf pressure / BMP085_P0 1.0f / 5.255f * 44330.0f data.Pressure = pressure / 1000.0f data.Temperature = PIOS_BMP085_GetTemperature / 10.0f BaroAltitudeSet &data return 0 static void updateAttitude AttitudeRawData * attitudeRaw float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001 : portMAX_DELAY & thisSysTime - lastSysTime / portTICK_RATE_MS / 1000.0f lastSysTime = thisSysTime float gyro 3 gyro 0 = attitudeRaw->gyros 0 gyro 1 = attitudeRaw->gyros 1 gyro 2 = attitudeRaw->gyros 2 float * accels = attitudeRaw->accels float grot 3 float accel_err 3 grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 CrossProduct const float * accels const float * grot accel_err float accel_mag = sqrt accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyro 0 += accel_err 0 * accelKp / dT gyro 1 += accel_err 1 * accelKp / dT gyro 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyro 0 - q 2 * gyro 1 - q 3 * gyro 2 * dT * M_PI / 180 / 2 qdot 1 = q 0 * gyro 0 - q 3 * gyro 1 + q 2 * gyro 2 * dT * M_PI / 180 / 2 qdot 2 = q 3 * gyro 0 + q 0 * gyro 1 - q 1 * gyro 2 * dT * M_PI / 180 / 2 qdot 3 = -q 2 * gyro 0 + q 1 * gyro 1 + q 0 * gyro 2 * dT * M_PI / 180 / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrt q 0 *q 0 + q 1 *q 1 + q 2 *q 2 + q 3 *q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabs qmag < 1e-3 || qmag != qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb UAVObjEvent * objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1
LibrePilot 4443 attitude.c Control 59507249e1 Hack in attitude to read directly from gyros instead of fifo. All sensorsreading now. */ PIOS_MPU6050_ReadGyros &gyro gyro_accum 0 = gyro.gyro_x gyro_accum 1 = gyro.gyro_y gyro_accum 2 = gyro.gyro_z gyro_samples = 1
LibrePilot 4444 attitude.c Control aeb42332d6 Get the MPU6050 fifo running and read by the attitude module
LibrePilot 4447 attitude.c Control 1deb799b6f Get simple attitude estimation working on F4 INS. Also make MPU6050 returnscaling that casts to degrees. #define STACK_SIZE_BYTES 1540 #define F_PI 3.14159265358979323846f #define PI_MOD x fmod x + F_PI F_PI * 2 - F_PI updateAttitude &attitudeRaw attitudeRaw->accels ATTITUDERAW_ACCELS_Y = -accel_accum 1 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Z = -accel_accum 2 * scaling float accel_mag float qmag float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float accels 3 accels 0 = attitudeRaw->accels 0 accels 1 = attitudeRaw->accels 1 accels 2 = attitudeRaw->accels 2 float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 CrossProduct const float * accels const float * grot accel_err accel_mag = accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyro 0 += accel_err 0 * accelKp / dT gyro 1 += accel_err 1 * accelKp / dT gyro 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyro 0 - q 2 * gyro 1 - q 3 * gyro 2 * dT * F_PI / 180 / 2 qdot 1 = q 0 * gyro 0 - q 3 * gyro 1 + q 2 * gyro 2 * dT * F_PI / 180 / 2 qdot 2 = q 3 * gyro 0 + q 0 * gyro 1 - q 1 * gyro 2 * dT * F_PI / 180 / 2 qdot 3 = -q 2 * gyro 0 + q 1 * gyro 1 + q 0 * gyro 2 * dT * F_PI / 180 / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 *q 0 + q 1 *q 1 + q 2 *q 2 + q 3 *q 3 if fabs qmag < 1.0e-3f || qmag != qmag
LibrePilot 4453 attitude.c Control 0efffef1fa Work on bringing the Revolution board up gyro_test = PIOS_MPU6000_Test struct pios_mpu6000_data gyro while read_good = PIOS_MPU6000_ReadFifo &gyro != 0 read_good = PIOS_MPU6000_ReadFifo &gyro scaling = PIOS_MPU6000_GetScale / gyro_samples
LibrePilot 4459 attitude.c Control 6d74e96c31 Got attitude working again. MPU6000 FIFO reading needs a bit of work. attitudeRaw->accels ATTITUDERAW_ACCELS_X = accel_accum 1 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Y = accel_accum 0 * scaling attitudeRaw->gyros ATTITUDERAW_GYROS_X = float gyro_accum 1 * scaling attitudeRaw->gyros ATTITUDERAW_GYROS_Y = float gyro_accum 0 * scaling
LibrePilot 4462 attitude.c Control a7ef5601e0 Update the MPU6000 FIFO code and also fix the temperature code attitudeRaw->temperature ATTITUDERAW_TEMPERATURE_GYRO = 35.0f + float gyro.temperature + 512.0f / 340.0f attitudeRaw->temperature ATTITUDERAW_TEMPERATURE_ACCEL = 25.0f + float accel.temperature - 2.0f / 2.0f
LibrePilot 4465 attitude.c Non-Control 7637e9f669 Get flash chip working and saving to memory. Also get the I2C working withBMA180 driver.
LibrePilot 4476 attitude.c Control 8b7aca0dcd Include the accel bias into revolution attitude function and also make suredownsampling is correct. float accels 3 = float accel_accum 1 / accel_samples float accel_accum 0 / accel_samples - float accel_accum 2 / accel_samples scaling = PIOS_BMA180_GetScale attitudeRaw->accels ATTITUDERAW_ACCELS_X = accels 0 - accelbias 0 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Y = accels 1 - accelbias 1 * scaling attitudeRaw->accels ATTITUDERAW_ACCELS_Z = accels 2 - accelbias 2 * scaling gyro_samples = count float gyros 3 = float gyro_accum 1 / gyro_samples float gyro_accum 0 / gyro_samples - float gyro_accum 2 / gyro_samples scaling = PIOS_MPU6000_GetScale attitudeRaw->gyros ATTITUDERAW_GYROS_X = gyros 0 * scaling attitudeRaw->gyros ATTITUDERAW_GYROS_Y = gyros 1 * scaling attitudeRaw->gyros ATTITUDERAW_GYROS_Z = gyros 2 * scaling
LibrePilot 4497 attitude.c Control f7d13ebd57 Hack to tweak the gyro gain for now although its too far out at the moment sosomething isn t configured properly. Possibly it is staying in 250 deg/s mode.Also make sure if the MPU6000 fifo backs up to pull extra data. scaling = gyroGain / 0.42 attitudeRaw->gyros ATTITUDERAW_GYROS_X *= scaling attitudeRaw->gyros ATTITUDERAW_GYROS_Y *= scaling attitudeRaw->gyros ATTITUDERAW_GYROS_Z *= scaling static int32_t timeval float dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw
LibrePilot 4514 attitude.c Control a9c61845af Create a separate task for sensors and attitude on revo. static xTaskHandle sensorTaskHandle static xTaskHandle attitudeTaskHandle static xQueueHandle gyroQueue static xQueueHandle accelQueue static xQueueHandle magQueue const uint32_t SENSOR_QUEUE_SIZE = 10 static void SensorTask void *parameters static void updateAttitude struct gyro_data float x float y float z struct accel_data float x float y float z struct mag_data float x float y float z gyroQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof struct gyro_data accelQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof struct accel_data magQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof struct mag_data xTaskCreate SensorTask signed char * Sensors STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &sensorTaskHandle xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &attitudeTaskHandle TaskMonitorAdd TASKINFO_RUNNING_SENSORS sensorTaskHandle TaskMonitorAdd TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_SENSORS * The sensor task. This polls the gyros at 500 Hz and pumps that data to * stabilization and to the attitude loop static void SensorTask void *parameters portTickType lastSysTime AlarmsClear SYSTEMALARMS_ALARM_SENSORS if accel_test < 0 || gyro_test < 0 || mag_test < 0 AlarmsSet SYSTEMALARMS_ALARM_SENSORS SYSTEMALARMS_ALARM_CRITICAL while 1 PIOS_WDG_UpdateFlag PIOS_WDG_SENSORS vTaskDelay 1 lastSysTime = xTaskGetTickCount PIOS_WDG_UpdateFlag PIOS_WDG_SENSORS vTaskDelayUntil &lastSysTime 2 / portTICK_RATE_MS static void AttitudeTask void *parameters uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb AttitudeSettingsHandle while 1 updateAttitude PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE struct accel_data accel_data accel_data.x = attitudeRaw->accels ATTITUDERAW_ACCELS_X accel_data.y = attitudeRaw->accels ATTITUDERAW_ACCELS_X accel_data.z = attitudeRaw->accels ATTITUDERAW_ACCELS_X if xQueueSendToBack accelQueue void * &accel_data 0 != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING struct gyro_data gyro_data gyro_data.x = attitudeRaw->gyros ATTITUDERAW_GYROS_X gyro_data.y = attitudeRaw->gyros ATTITUDERAW_GYROS_Y gyro_data.z = attitudeRaw->gyros ATTITUDERAW_GYROS_Z if xQueueSendToBack gyroQueue void * &gyro_data 0 != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING static void updateAttitude struct gyro_data gyro_data struct accel_data accel_data if xQueueReceive gyroQueue void * &gyro_data 10 / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue void * &accel_data 10 / portTICK_RATE_MS != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return CrossProduct const float * &accel_data.x const float * grot accel_err accel_mag = accel_data.x*accel_data.x + accel_data.y*accel_data.y + accel_data.z*accel_data.z gyro_data.x += accel_err 0 * accelKp / dT gyro_data.y += accel_err 1 * accelKp / dT gyro_data.z += accel_err 2 * accelKp / dT qdot 0 = -q 1 * gyro_data.x - q 2 * gyro_data.y - q 3 * gyro_data.z * dT * F_PI / 180 / 2 qdot 1 = q 0 * gyro_data.x - q 3 * gyro_data.y + q 2 * gyro_data.z * dT * F_PI / 180 / 2 qdot 2 = q 3 * gyro_data.x + q 0 * gyro_data.y - q 1 * gyro_data.z * dT * F_PI / 180 / 2 qdot 3 = -q 2 * gyro_data.x + q 1 * gyro_data.y + q 0 * gyro_data.z * dT * F_PI / 180 / 2 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE
LibrePilot 4516 attitude.c Non-Control d22e0e66dd Create separate UAVOs for the sensors. #include magnetometer.h #include accels.h #include gyros.h gyroQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof GyrosData accelQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof AccelsData magQueue = xQueueCreate SENSOR_QUEUE_SIZE sizeof MagnetometerData
LibrePilot 4517 attitude.c Control 53cb5b67d1 Make the revo attitude now use the separate gyro object. CopterControlattitude still needs an update. static int32_t updateSensors static int32_t updateAttitudeComplimentary static float gyro_bias 3 = 0 0 0 GyrosInitialize AccelsInitialize MagnetometerInitialize gyro_bias 0 = 0 gyro_bias 1 = 0 gyro_bias 2 = 0 if updateSensors != 0 updateAttitudeComplimentary static int32_t updateSensors AccelsData accelsData accelsData.x = accels 0 - accelbias 0 * scaling accelsData.y = accels 1 - accelbias 1 * scaling accelsData.z = accels 2 - accelbias 2 * scaling accelsData.temperature = 25.0f + float accel.temperature - 2.0f / 2.0f AccelsSet &accelsData if xQueueSendToBack accelQueue void * &accelsData 0 != pdTRUE GyrosData gyrosData gyrosData.x = gyros 0 * scaling gyrosData.y = gyros 1 * scaling gyrosData.z = gyros 2 * scaling gyrosData.temperature = 35.0f + float gyro.temperature + 512.0f / 340.0f if xQueueSendToBack gyroQueue void * &gyrosData 0 != pdTRUE gyrosData.x += gyro_bias 0 gyrosData.y += gyro_bias 1 gyrosData.z += gyro_bias 2 GyrosSet &gyrosData gyro_bias 2 += - gyrosData.z * yawBiasRate MagnetometerData mag mag.x = -values 0 mag.y = -values 1 mag.z = -values 2 MagnetometerSet &mag static int32_t updateAttitudeComplimentary GyrosData gyrosData AccelsData accelsData if xQueueReceive gyroQueue void * &gyrosData 10 / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue void * &accelsData 10 / portTICK_RATE_MS != pdTRUE return -1 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z gyro_bias 0 += accel_err 0 * accelKi gyro_bias 1 += accel_err 1 * accelKi gyrosData.x += accel_err 0 * accelKp / dT gyrosData.y += accel_err 1 * accelKp / dT gyrosData.z += accel_err 2 * accelKp / dT qdot 0 = -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT * F_PI / 180 / 2 qdot 1 = q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT * F_PI / 180 / 2 qdot 2 = q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT * F_PI / 180 / 2 qdot 3 = -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT * F_PI / 180 / 2 return 0 gyro_bias 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_bias 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_bias 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f
LibrePilot 4519 attitude.c Non-Control a37a17a4fb Created a separate Sensor module and Attitude module for revolution #include gyrosbias.h #define FAILSAFE_TIMEOUT_MS 10 static int32_t updateAttitudeComplimentary bool first_run * Initialise the module. Called before the start function GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0 gyrosBias.y = 0 gyrosBias.z = 0 GyrosBiasSet &gyrosBias * Start the task. Expects all objects to be initialized by this point. * \returns 0 on success or -1 if initialisation failed int32_t AttitudeStart void gyroQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES/4 NULL TASK_PRIORITY &attitudeTaskHandle TaskMonitorAdd TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue return 0 MODULE_INITCALL AttitudeInitialize AttitudeStart bool first_run = true updateAttitudeComplimentary first_run if first_run first_run = false PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE static int32_t updateAttitudeComplimentary bool first_run UAVObjEvent ev static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_STABILIZATION SYSTEMALARMS_ALARM_WARNING FlightStatusData flightStatus FlightStatusGet &flightStatus if first_run init = 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1 accelKi = 0.9 yawBiasRate = 0.23 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1 accelKi = 0.9 yawBiasRate = 0.23 init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate init = 1 GyrosGet &gyrosData AccelsGet &accelsData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x += accel_err 0 * accelKi gyrosBias.y += accel_err 1 * accelKi gyrosBias.z += - gyrosData.z * yawBiasRate GyrosBiasSet &gyrosBias GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyrosBias.y = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyrosBias.z = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f GyrosBiasSet &gyrosBias
LibrePilot 4523 attitude.c Control fd8899018f Get EKF running #include baroaltitude.h #include homelocation.h #define STACK_SIZE_BYTES 5540 static xQueueHandle baroQueue static int32_t updateAttitudeINSGPS bool first_run accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue BaroAltitudeConnectQueue baroQueue vTaskDelay 100 if 0 updateAttitudeComplimentary first_run else updateAttitudeINSGPS first_run AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING #include insgps.h int32_t ins_failed = 0 extern struct NavStruct Nav static int32_t updateAttitudeINSGPS bool first_run UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData BaroAltitudeData baroData static uint32_t ins_last_time = 0 static bool inited if first_run inited = false if xQueueReceive gyroQueue &ev 10 / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 10 / portTICK_RATE_MS != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData bool mag_updated bool baro_updated if inited mag_updated = 0 baro_updated = 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !inited && !mag_updated || !baro_updated return -1 else if !inited inited = true float Rbe 3 3 q 4 accels 3 rpy 3 mag float ge 3 = 0.0f 0.0f -9.81f float zeros 3 = 0.0f 0.0f 0.0f float Pdiag 16 = 25.0f 25.0f 25.0f 5.0f 5.0f 5.0f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-4f 1e-4f 1e-4f bool using_mags using_gps INSGPSInit HomeLocationData home HomeLocationGet &home RotFrom2Vectors &accelsData.x ge &magData.x home.Be Rbe R2Quaternion Rbe q INSSetState zeros zeros q zeros zeros INSResetP Pdiag ins_last_time = PIOS_DELAY_GetRaw return 0 static uint32_t updated_without_gps = 0 float zeros 3 = 0 0 0 uint16_t sensors = 0 float dT dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f float gyros 3 = gyrosData.x * F_PI / 180.0f gyrosData.y * F_PI / 180.0f gyrosData.z * F_PI / 180.0f INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT sensors = HORIZ_SENSORS | VERT_SENSORS if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSCorrection &magData.x zeros zeros baroData.Altitude sensors if fabs Nav.gyro_bias 0 > 0.1f || fabs Nav.gyro_bias 1 > 0.1f || fabs Nav.gyro_bias 2 > 0.1f float zeros 3 = 0.0f 0.0f 0.0f INSSetGyroBias zeros static void settingsUpdatedCb UAVObjEvent * objEv
LibrePilot 4524 attitude.c Control 09864a8cf6 Get outdoor EKF running although need to add in the ability to swap modes(probably). #include positionactual.h #include velocityactual.h #include gpsposition.h static xQueueHandle gpsQueue PositionActualInitialize VelocityActualInitialize gpsQueue = xQueueCreate 1 sizeof UAVObjEvent GPSPositionConnectQueue gpsQueue bool gps_updated gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !inited && !mag_updated || !baro_updated || !gps_updated float vel 3 NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition vel 0 = gpsPosition.Groundspeed * cosf gpsPosition.Heading * F_PI / 180.0f vel 1 = gpsPosition.Groundspeed * sinf gpsPosition.Heading * F_PI / 180.0f vel 2 = 0 float LLA 3 = float gpsPosition.Latitude / 1e7f float gpsPosition.Longitude / 1e7f float gpsPosition.GeoidSeparation + gpsPosition.Altitude float ECEF 3 = float home.ECEF 0 / 100.0f float home.ECEF 1 / 100.0f float home.ECEF 2 / 100.0f LLA2Base LLA ECEF float * 3 home.RNE NED INSSetState NED vel q &gyrosData.x zeros INSSetGyroBias &gyrosData.x GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias float gyros 3 = gyrosData.x + gyrosBias.x * F_PI / 180.0f gyrosData.y + gyrosBias.y * F_PI / 180.0f gyrosData.z + gyrosBias.z * F_PI / 180.0f gyrosBias.x = Nav.gyro_bias 0 gyrosBias.y = Nav.gyro_bias 1 gyrosBias.z = Nav.gyro_bias 2 GyrosBiasSet &gyrosBias float NED 3 = 0 0 0 float vel 3 = 0 0 0 if gps_updated sensors = HORIZ_SENSORS | VERT_SENSORS GPSPositionData gpsPosition GPSPositionGet &gpsPosition vel 0 = gpsPosition.Groundspeed * cosf gpsPosition.Heading * F_PI / 180.0f vel 1 = gpsPosition.Groundspeed * sinf gpsPosition.Heading * F_PI / 180.0f vel 2 = 0 HomeLocationData home HomeLocationGet &home float LLA 3 = float gpsPosition.Latitude / 1e7f float gpsPosition.Longitude / 1e7f float gpsPosition.GeoidSeparation + gpsPosition.Altitude float ECEF 3 = float home.ECEF 0 / 100.0f float home.ECEF 1 / 100.0f float home.ECEF 2 / 100.0f LLA2Base LLA ECEF float * 3 home.RNE NED INSCorrection &magData.x NED vel baroData.Altitude sensors PositionActualData positionActual PositionActualGet &positionActual positionActual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual
LibrePilot 4527 attitude.c Control 43ed6cd89c Need to initialize the HomeLocation object HomeLocationInitialize
LibrePilot 4529 attitude.c Control 0432dd450e Enable the mag to be used in the complimentary filter to stabilize the yawdirection if 1 float attitudeDt float mag_err 3 float magKi = 0.000001f float magKp = 0.0001f if 1 float brot 3 float Rbe 3 3 MagnetometerData mag HomeLocationData home Quaternion2R q Rbe MagnetometerGet &mag HomeLocationGet &home rot_mult Rbe home.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1 || mag_len < 1 mag_err 0 = mag_err 1 = mag_err 2 = 0 else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0 gyrosBias.z += mag_err 2 * magKi gyrosData.z += accel_err 2 * accelKp / dT + mag_err 2 * magKp / dT
LibrePilot 4531 attitude.c Control 0b00e748a9 OP-565: Added the ability to do a trim flight to compute the accels for level flight #include manualcontrolcommand.h static volatile bool trim_requested = false static volatile int32_t trim_accels 3 static volatile int32_t trim_samples int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535 #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f trim_requested = false if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0 trim_samples++ trim_accels 0 += attitudeRaw->accels ATTITUDERAW_ACCELS_X trim_accels 1 += attitudeRaw->accels ATTITUDERAW_ACCELS_Y trim_accels 2 += attitudeRaw->accels ATTITUDERAW_ACCELS_Z attitudeRaw->accels ATTITUDERAW_ACCELS_X = attitudeRaw->accels ATTITUDERAW_ACCELS_X - accelbias 0 * ACCEL_SCALE attitudeRaw->accels ATTITUDERAW_ACCELS_Y = attitudeRaw->accels ATTITUDERAW_ACCELS_Y - accelbias 1 * ACCEL_SCALE attitudeRaw->accels ATTITUDERAW_ACCELS_Z = attitudeRaw->accels ATTITUDERAW_ACCELS_Z - accelbias 2 * ACCEL_SCALE if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false * @ * @ */
LibrePilot 4885 attitude.c Control a20d654744 Update the attitude algorithm for CC to use the Gyros and Accels UAVObjectsinstead of AttitudeRaw #include gyros.h #include accels.h static int8_t updateSensors AccelsData * GyrosData * static void updateAttitude AccelsData * GyrosData * AccelsInitialize GyrosInitialize AccelsData accels GyrosData gyros if updateSensors &accels &gyros != 0 updateAttitude &accels &gyros AccelsSet &accels GyrosSet &gyros static int8_t updateSensors AccelsData * accels GyrosData * gyros gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain gyros->temperature = samples_remaining accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 2 += - gyros->z * yawBiasRate static void updateAttitude AccelsData * accelsData GyrosData * gyrosData float * gyros = &gyrosData->x float * accels = &accelsData->x float grot 3 float accel_err 3 grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 CrossProduct const float * accels const float * grot accel_err float accel_mag = sqrt accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI / 180 / 2 qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI / 180 / 2 qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI / 180 / 2 qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI / 180 / 2
LibrePilot 4887 attitude.c Control 11471ff68b Update drivers for CC to the new EXTI system and SPI calls
LibrePilot 4892 attitude.c Control 728c05f973 Crude implementation of sensor reading for CC3D #include static int32_t updateSensors AccelsData * GyrosData * static int32_t updateSensorsCC3D AccelsData * accelsData GyrosData * gyrosData const struct pios_board_info * bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 int32_t retval if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 static int32_t updateSensors AccelsData * accels GyrosData * gyros struct pios_bma180_data accel struct pios_l3gd20_data gyro static int32_t updateSensorsCC3D AccelsData * accelsData GyrosData * gyrosData static portTickType lastSysTime int32_t read_good int32_t count = 0 bool error = false uint32_t accel_samples uint32_t gyro_samples int32_t accel_accum 3 = 0 0 0 int32_t gyro_accum 3 = 0 0 0 float gyro_scaling float accel_scaling while read_good = PIOS_BMA180_ReadFifo &accel != 0 && !error error = xTaskGetTickCount - lastSysTime > 5 ? true : error if error struct pios_bma180_data data PIOS_BMA180_ReadAccels &data lastSysTime = xTaskGetTickCount return -1 while read_good == 0 count++ accel_accum 0 += accel.x accel_accum 1 += accel.y accel_accum 2 += accel.z read_good = PIOS_BMA180_ReadFifo &accel accel_samples = count accel_scaling = PIOS_BMA180_GetScale count = 0 while read_good = PIOS_L3GD20_ReadFifo &gyro != 0 && !error error = xTaskGetTickCount - lastSysTime > 5 ? true : error while read_good == 0 count++ gyro_accum 0 += gyro.gyro_x gyro_accum 1 += gyro.gyro_y gyro_accum 2 += gyro.gyro_z read_good = PIOS_L3GD20_ReadFifo &gyro gyro_samples = count gyro_scaling = PIOS_L3GD20_GetScale float accels 3 = float accel_accum 1 / accel_samples float accel_accum 0 / accel_samples - float accel_accum 2 / accel_samples accelsData->x = accels 0 * accel_scaling - accelbias 0 accelsData->y = accels 1 * accel_scaling - accelbias 1 accelsData->z = accels 2 * accel_scaling - accelbias 2 float gyros 3 = float gyro_accum 1 / gyro_samples float gyro_accum 0 / gyro_samples - float gyro_accum 2 / gyro_samples gyrosData->x = gyros 0 * gyro_scaling gyrosData->y = gyros 1 * gyro_scaling gyrosData->z = gyros 2 * gyro_scaling lastSysTime = xTaskGetTickCount return 0
LibrePilot 4894 attitude.c Non-Control 4d3af7afbf Add ID test to ADXL345 int32_t adxl_test adxl_test = PIOS_ADXL345_Test
LibrePilot 4898 attitude.c Control 486cdb5dbb Got CC running again although with altitude and bmp085 disabled as well as theCC3D drivers. uint32_t accel_samples = 0 uint32_t gyro_samples = 0 float gyro_scaling = 1 float accel_scaling = 1 #if defined PIOS_INCLUDE_BMA180 accel_samples = 0 bool error = false int32_t accel_read_good struct pios_bma180_data accel while accel_read_good = PIOS_BMA180_ReadFifo &accel != 0 && !error while accel_read_good == 0 accel_samples++ accel_read_good = PIOS_BMA180_ReadFifo &accel #endif #if defined PIOS_INCLUDE_L3GD20 int32_t gyro_read_good gyro_samples = 0 struct pios_l3gd20_data gyro bool gyro_error = false while gyro_read_good = PIOS_L3GD20_ReadFifo &gyro != 0 && !gyro_error gyro_error = xTaskGetTickCount - lastSysTime > 5 ? true : gyro_error while gyro_read_good == 0 gyro_samples++ gyro_read_good = PIOS_L3GD20_ReadFifo &gyro #endif
LibrePilot 4901 attitude.c Control e9552065a9 Reenable the CC3D sensors while gyro_read_good = PIOS_L3GD20_ReadFifo &gyro != 0 && !gyro_error
LibrePilot 4902 attitude.c Control 8e79bc2ee2 Temporarily refactor attitude to directly read the sensors instead of pull fromtheir queues until the interrupts are working properly #define UPDATE_RATE 25.0f int32_t accel_test int32_t gyro_test portTickType lastSysTime if cc3d #if defined PIOS_INCLUDE_BMA180 accel_test = PIOS_BMA180_Test #endif #if defined PIOS_INCLUDE_L3GD20 gyro_test = PIOS_L3GD20_Test #endif else #if defined PIOS_INCLUDE_ADXL345 accel_test = PIOS_ADXL345_Test #endif gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE lastSysTime = xTaskGetTickCount if cc3d vTaskDelayUntil &lastSysTime 5 struct pios_bma180_data accel struct pios_l3gd20_data gyro if PIOS_BMA180_ReadAccels &accel < 0 accel_accum 0 += accel.x accel_accum 1 += accel.y accel_accum 2 += accel.z accel_samples = 1 if PIOS_L3GD20_ReadGyros &gyro < 0 return -1 gyro_accum 0 += gyro.gyro_x gyro_accum 1 += gyro.gyro_y gyro_accum 2 += gyro.gyro_z gyro_samples =1 float accel_mag = sqrtf accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 float qmag = sqrtf q 0 *q 0 + q 1 *q 1 + q 2 *q 2 + q 3 *q 3
LibrePilot 4905 attitude.c Control 8c208412c9 Give the BMA180 a dynamically allocated device structure and also make itproperly bind to a SPI and slave number. Temporarily reducing spi bus toslowest possible speed. if PIOS_BMA180_Test < 0 PIOS_BMA180_ReadAccels &accel
LibrePilot 4909 attitude.c Control 5efbfb02fa Make the BMA180 sensor system acquire from the queue. Correct the orientationand speed it up to 10 MHz communications. accel_samples = 0 bool error = false int32_t accel_read_good while accel_read_good = PIOS_BMA180_ReadFifo &accel != 0 && !error error = xTaskGetTickCount - lastSysTime > 5 ? true : error if error struct pios_bma180_data data PIOS_BMA180_ReadAccels &data lastSysTime = xTaskGetTickCount while accel_read_good == 0 accel_samples++ accel_accum 0 += -accel.x accel_accum 1 += -accel.y accel_accum 2 += accel.z accel_read_good = PIOS_BMA180_ReadFifo &accel
LibrePilot 4914 attitude.c Control b6c056fef2 Use the updated L3GD20 format and make attitude.c use the gyro data from thefifo PIOS_BMA180_ReadAccels &accel accel_accum 0 += accel.x accel_accum 1 += accel.y int32_t gyro_read_good gyro_samples = 0 bool gyro_error = false while gyro_read_good = PIOS_L3GD20_ReadFifo &gyro != 0 && !gyro_error gyro_error = xTaskGetTickCount - lastSysTime > 5 ? true : gyro_error if gyro_error PIOS_L3GD20_ReadGyros &gyro lastSysTime = xTaskGetTickCount while gyro_read_good == 0 gyro_samples++ gyro_accum 0 += gyro.gyro_x gyro_accum 1 += gyro.gyro_y gyro_accum 2 += gyro.gyro_z gyro_read_good = PIOS_L3GD20_ReadFifo &gyro float accels 3 = float -accel_accum 1 / accel_samples float -accel_accum 0 / accel_samples - float accel_accum 2 / accel_samples float gyros 3 = float gyro_accum 0 / gyro_samples float -gyro_accum 1 / gyro_samples - float gyro_accum 2 / gyro_samples
LibrePilot 4920 attitude.c Control a25d52785b Change the bus speed for the L3GD20 to 10MHz and some small cleanups struct pios_bma180_data accel struct pios_l3gd20_data gyro
LibrePilot 4923 attitude.c Control 104c091590 Correct the range constants for the L3GD20 driver vTaskDelayUntil &lastSysTime 3
LibrePilot 4938 attitude.c Control 205aabe895 L3GD20: Make this sensor push data onto a FreeRTOS queue so that the sensorreading task can block according. Also now the gyro data is sent tostabilization multiple times per update of the attitude loop. float gyros_passed 3 gyros_passed 0 = gyros->x gyros_passed 1 = gyros->y gyros_passed 2 = gyros->z float gyros_accum 3 = 0 0 0 #if defined PIOS_INCLUDE_L3GD20 xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue struct pios_l3gd20_data gyro gyro_scaling = PIOS_L3GD20_GetScale if xQueueReceive gyro_queue void * &gyro 3 == errQUEUE_EMPTY PIOS_L3GD20_ReadGyros &gyro AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 gyrosData->x = gyro.gyro_x * gyro_scaling gyrosData->y = -gyro.gyro_y * gyro_scaling gyrosData->z = -gyro.gyro_z * gyro_scaling gyrosData->temperature = 0 GyrosSet gyrosData gyros_accum 0 = gyrosData->x gyros_accum 1 = gyrosData->y gyros_accum 2 = gyrosData->z if xQueueReceive gyro_queue void * const &gyro 3 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 gyrosData->x = gyro.gyro_x * gyro_scaling gyrosData->y = -gyro.gyro_y * gyro_scaling gyrosData->z = -gyro.gyro_z * gyro_scaling gyrosData->temperature = 0 gyros_accum 0 += gyrosData->x gyros_accum 1 += gyrosData->y gyros_accum 2 += gyrosData->z if xQueueReceive gyro_queue void * const &gyro 3 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 gyrosData->x = gyro.gyro_x * gyro_scaling gyrosData->y = -gyro.gyro_y * gyro_scaling gyrosData->z = -gyro.gyro_z * gyro_scaling gyrosData->temperature = 0 gyros_accum 0 += gyrosData->x gyros_accum 1 += gyrosData->y gyros_accum 2 += gyrosData->z gyros_passed 0 = gyros_accum 0 / 3 gyros_passed 1 = gyros_accum 1 / 3 gyros_passed 2 = gyros_accum 2 / 3 #endif float * gyros = gyros_passed
LibrePilot 4942 attitude.c Control 727f0befc5 Make the gyro go a bit more regularly to stabilization but needs updatingregular CC if xQueueReceive gyro_queue void * &gyro 2 == errQUEUE_EMPTY if xQueueReceive gyro_queue void * const &gyro 2 == errQUEUE_EMPTY if xQueueReceive gyro_queue void * const &gyro 2 == errQUEUE_EMPTY GyrosSet gyrosData
LibrePilot 4979 attitude.c Control 84dd1dbc40 A bit more lenient settings for CC sensor timings if xQueueReceive gyro_queue void * &gyro 3 == errQUEUE_EMPTY if xQueueReceive gyro_queue void * const &gyro 3 == errQUEUE_EMPTY if xQueueReceive gyro_queue void * const &gyro 3 == errQUEUE_EMPTY
LibrePilot 5012 attitude.c Control 665801662d In cc3d update sensors add accelbias before scaling to allow more precision inlevelinIn cc3d update sensors add accelbias before scaling to allow moreprecision in levelingg accelsData->x = accels 0 - accelbias 0 * accel_scaling accelsData->y = accels 1 - accelbias 1 * accel_scaling accelsData->z = accels 2 - accelbias 2 * accel_scaling
LibrePilot 5092 attitude.c Non-Control e2939dae2a revo usb: Add support for USB to Revo
LibrePilot 5108 attitude.c Non-Control 02bec750bd In the attitude loop need to check all the queues even if unused or the eventsystem will have warnings if xQueueReceive accelQueue &ev 0 != pdTRUE AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if xQueueReceive magQueue &ev 0 != pdTRUE if xQueueReceive baroQueue &ev 0 != pdTRUE if xQueueReceive gpsQueue &ev 0 != pdTRUE
LibrePilot 5235 attitude.c Non-Control d45e88515b firmware: disable Attitude module in simulationthis part should be rewritten to completely disable the polling of sensors.i just do not know how to do it correctly. if AttitudeRawReadOnly == ACCESS_READONLY return 0 if AttitudeActualReadOnly == ACCESS_READONLY return
LibrePilot 5270 attitude.c Non-Control 0d42303d37 Fix some compilation errors/warnings in attitude.c float Rbe 3 3 q 4 return 0
LibrePilot 5315 attitude.c Control d32dd0ccd4 Fixed fmod() versus fmodf() usage in flight/... #define PI_MOD x fmodf x + F_PI F_PI * 2 - F_PI
LibrePilot 5324 attitude.c Control 1415728762 Get the MPU6000 CC3D working. Right now the ADC system is commented out whichwill break regular CC. #define SENSOR_PERIOD 2 #if defined PIOS_INCLUDE_MPU6000 gyro_test = PIOS_MPU6000_Test #if defined PIOS_INCLUDE_ADC #endif GyrosSet gyros AccelsSet accels struct pios_mpu6000_data gyro float gyro_accum 3 = 0 0 0 uint32_t gyro_samples vTaskDelayUntil &lastSysTime SENSOR_PERIOD / portTICK_RATE_MS #if defined PIOS_INCLUDE_MPU6000 uint32_t count = 0 int32_t read_good = 0 int32_t error = false while read_good = PIOS_MPU6000_ReadFifo &gyro != 0 && !error error = xTaskGetTickCount - lastSysTime > SENSOR_PERIOD ? true : error if error while read_good == 0 count++ gyro_accum 0 += gyro.gyro_x gyro_accum 1 += gyro.gyro_y gyro_accum 2 += gyro.gyro_z accel_accum 0 += gyro.accel_x accel_accum 1 += gyro.accel_y accel_accum 2 += gyro.accel_z read_good = PIOS_MPU6000_ReadFifo &gyro gyro_samples = count gyro_scaling = PIOS_MPU6000_GetScale accel_samples = count accel_scaling = PIOS_MPU6000_GetAccelScale #endif gyrosData->temperature = 35.0f + float gyro.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float gyro.temperature + 512.0f / 340.0f float accels 3 = - float accel_accum 1 / accel_samples - float accel_accum 0 / accel_samples - float accel_accum 2 / accel_samples float gyros 3 = - float gyro_accum 1 / gyro_samples - float gyro_accum 0 / gyro_samples - float gyro_accum 2 / gyro_samples if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 AccelsSet &accelsData gyrosData->x = gyros 0 * gyro_scaling gyrosData->y = gyros 1 * gyro_scaling gyrosData->z = gyros 2 * gyro_scaling if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 GyrosSet gyrosData AccelsSet accelsData float * gyros = &gyrosData->x
LibrePilot 5328 attitude.c Control a960f3d07e Switch the MPU6000 driver to using a FreeRTOS queue. Need to update Revo codeto take advantage of this. struct pios_mpu6000_data mpu6000_data float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 gyros 0 = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 1 = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 2 = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 1 = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 2 = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2
LibrePilot 5332 attitude.c Control 8032130769 Slow down gyro a bit on CC to give the CPU some breathing room. Might need tocome down more. if accel_mag < 1.0e-3f return
LibrePilot 5335 attitude.c Control 3346e848e0 Attitude error timeout was 2 ms when sensors at 500 Hz. Caused lots of errorsand overwhelmed event system. #define SENSOR_PERIOD 4
LibrePilot 5346 attitude.c Control 55ee568fe2 Work on the INS algorithm #include gpsposition.h if 0 bool outdoor_mode = false int32_t init_stage = 0 GPSPositionData gpsData GyrosBiasData gyrosBias static bool mag_updated static bool baro_updated static bool gps_updated static uint32_t ins_last_time = 0 float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT inited = first_run ? false : inited gps_updated = 0 gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData GPSPositionGet &gpsData gps_updated &= gpsData.Satellites >= 7 && gpsData.PDOP >= 3.0f if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode if init_stage == 0 && !outdoor_mode float Pdiag 16 = 25.0f 25.0f 25.0f 5.0f 5.0f 5.0f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-4f 1e-4f 1e-4f float q 4 float var 3 = 500.0f 500.0f 500.0f float pos 3 = 0.0f 0.0f 0.0f pos 2 = baroData.Altitude * -1.0f INSGPSInit var 0 = var 1 = var 2 = 5e-3f INSSetMagVar var var 0 = var 1 = var 2 = 1.5e-5f INSSetAccelVar var var 0 = var 1 = var 2 = 2.0e-4f INSSetGyroVar var float rpy 3 rpy 0 = atan2f accelsData.x accelsData.z * 180.0f / F_PI rpy 1 = atan2f accelsData.y accelsData.z * 180.0f / F_PI rpy 2 = atan2f magData.x -magData.y * 180.0f / F_PI RPY2Quaternion rpy q INSSetState pos zeros q zeros zeros INSResetP Pdiag else if init_stage == 0 && outdoor_mode float q 4 rpy 3 float Pdiag 16 = 25.0f 25.0f 25.0f 5.0f 5.0f 5.0f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-5f 1e-4f 1e-4f 1e-4f float NED 3 float var 3 INSGPSInit var 0 = var 1 = var 2 = 5e-3f INSSetMagVar var var 0 = var 1 = var 2 = 1.5e-5f INSSetAccelVar var var 0 = var 1 = var 2 = 2.0e-4f INSSetGyroVar var HomeLocationData home HomeLocationGet &home INSSetMagNorth home.Be GPSPositionData gpsPosition GPSPositionGet &gpsPosition float LLA 3 = float gpsPosition.Latitude / 1e7f float gpsPosition.Longitude / 1e7f float gpsPosition.GeoidSeparation + gpsPosition.Altitude float ECEF 3 = float home.ECEF 0 / 100.0f float home.ECEF 1 / 100.0f float home.ECEF 2 / 100.0f LLA2Base LLA ECEF float * 3 home.RNE NED rpy 0 = atan2f accelsData.x accelsData.z * 180.0f / F_PI rpy 1 = atan2f accelsData.y accelsData.z * 180.0f / F_PI rpy 2 = atan2f magData.x -magData.y * 180.0f / F_PI RPY2Quaternion rpy q INSSetState NED zeros q zeros zeros INSResetP Pdiag else if init_stage > 0 GyrosBiasGet &gyrosBias float gyros 3 = gyrosData.x + gyrosBias.x * F_PI / 180.0f gyrosData.y + gyrosBias.y * F_PI / 180.0f gyrosData.z + gyrosBias.z * F_PI / 180.0f INSStatePrediction gyros &accelsData.x 0.002f init_stage++ if init_stage > 500 inited = true return -1 if !inited return -1 GyrosBiasGet &gyrosBias HomeLocationData home HomeLocationGet &home INSSetMagNorth home.Be if gps_updated && outdoor_mode INSSetPosVelVar 1.0f 1.0f sensors = POS_SENSORS float LLA 3 = float gpsPosition.Latitude / 1e7f float gpsPosition.Longitude / 1e7f float gpsPosition.GeoidSeparation + gpsPosition.Altitude float ECEF 3 = float home.ECEF 0 / 100.0f float home.ECEF 1 / 100.0f float home.ECEF 2 / 100.0f else if !outdoor_mode vel 0 = vel 1 = vel 2 = 0 NED 0 = NED 1 = 0 NED 2 = baroData.Altitude sensors |= HORIZ_POS_SENSORS if sensors INSCorrection &magData.x NED vel baroData.Altitude sensors * @ * @ */
LibrePilot 5352 attitude.c Control 9e4651ab80 INS working on Revo HomeLocationData home HomeLocationGet &home var 0 = var 1 = 1e-4f var 2 = 1e-1f if init_stage > 10 if mag_updated sensors |= MAG_SENSORS INSSetPosVelVar 1e-2f 1e-2f sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS
LibrePilot 5354 attitude.c Control e3c36ac99a Change requires for GSP updated to be set to true (wrong sign on PDOP check) gps_updated &= gpsData.Satellites >= 7 && gpsData.PDOP <= 4.0f INSSetPosVelVar 1e-2f 1e-2f sensors = POS_SENSORS | HORIZ_SENSORS
LibrePilot 5355 attitude.c Control 1911c7360d New NEDPosition object which is the transformed GPS location #include nedposition.h NEDPositionInitialize NEDPositionData nedPos NEDPositionGet &nedPos nedPos.North = NED 0 nedPos.East = NED 1 nedPos.Down = NED 2 NEDPositionSet &nedPos
LibrePilot 5356 attitude.c Control a9aa6b696b Add object for setting the fusion algorithm #include attitude.h #include gyros.h #include gyrosbias.h #include homelocation.h #include magnetometer.h #include nedposition.h #include positionactual.h #include revosettings.h #include velocityactual.h static RevoSettingsData revoSettings static int32_t updateAttitudeINSGPS bool first_run bool outdoor_mode static uint32_t last_algorithm RevoSettingsConnectCallback &settingsUpdatedCb settingsUpdatedCb RevoSettingsHandle last_algorithm = 0xfffffff if last_algorithm != revoSettings.FusionAlgorithm last_algorithm = revoSettings.FusionAlgorithm first_run = true switch revoSettings.FusionAlgorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLIMENTARY: updateAttitudeComplimentary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR break first_run = false static int32_t updateAttitudeINSGPS bool first_run bool outdoor_mode
LibrePilot 5357 attitude.c Non-Control e8cc7748af Clean up some unused variables in attitude code static AttitudeSettingsData attitudeSettings bool first_run uint32_t last_algorithm attitudeSettings.AccelKp = 1 attitudeSettings.AccelKi = 0.9 attitudeSettings.YawBiasRate = 0.23 else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1 attitudeSettings.AccelKi = 0.9 attitudeSettings.YawBiasRate = 0.23 AttitudeSettingsGet &attitudeSettings gyrosBias.x += accel_err 0 * attitudeSettings.AccelKi gyrosBias.y += accel_err 1 * attitudeSettings.AccelKi gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT RevoSettingsGet &revoSettings
LibrePilot 5358 attitude.c Control 0eedaa1250 Change how we convert LLA to NED. Now it is done with a taylor expansionaround the home LLA coordinate to avoid the conversion into ECEF coordinates.This has the benefit of not requiring double precision math and uses lessoperations.Now we should remove the Rne and ECEF fields from HomeLocation as they areunused static HomeLocationData homeLocation static int32_t getNED GPSPositionData * gpsPosition float * NED HomeLocationConnectCallback &settingsUpdatedCb getNED &gpsPosition NED float T 3 const float DEG2RAD = 3.141592653589793f / 180.0f static int32_t getNED GPSPositionData * gpsPosition float * NED float dL 3 = gpsPosition->Latitude - homeLocation.Latitude / 10.0e6 * DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6 * DEG2RAD gpsPosition->Altitude - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 float lat lon alt HomeLocationGet &homeLocation lat = homeLocation.Latitude / 10.0e6f * DEG2RAD lon = homeLocation.Longitude / 10.0e6 * DEG2RAD alt = homeLocation.Altitude T 0 = alt+6.378137E6f T 1 = cosf lat * alt+6.378137E6f T 2 = -1.0f
LibrePilot 5361 attitude.c Control 4cadb7ec0e Forgot to initialize revo settings object RevoSettingsInitialize
LibrePilot 5362 attitude.c Control e32ab9342c Shuffle some stack sizes around #define STACK_SIZE_BYTES 3540
LibrePilot 5364 attitude.c Control de7fc81795 Fixes to the GPS LLA to NED translation #define STACK_SIZE_BYTES 2540 float dL 3 = gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f * DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f * DEG2RAD
LibrePilot 5365 attitude.c Control ce2d1f94f6 Better hardcoded constants for INS variances var 0 = var 1 = var 2 = 1.5e-6f var 0 = var 1 = 1e-4f var 2 = 1e-1f
LibrePilot 5366 attitude.c Control 71d0180d45 Use the sensor variances from object #include revocalibration.h static RevoCalibrationData revoCalibration static RevoSettingsData revoSettings INSSetMagVar revoCalibration.mag_var INSSetAccelVar revoCalibration.accel_var INSSetGyroVar revoCalibration.gyro_var INSSetMagVar revoCalibration.mag_var INSSetAccelVar revoCalibration.accel_var INSSetGyroVar revoCalibration.gyro_var RevoCalibrationGet &revoCalibration INSSetMagVar revoCalibration.mag_var INSSetAccelVar revoCalibration.accel_var INSSetGyroVar revoCalibration.gyro_var
LibrePilot 5367 attitude.c Control 23625904c5 Remove the now unused ECEF and Rne fields from HomeLocation. Also reenablecalculation of the WMM and HomeLocation from the GPS code for revolution. getNED &gpsPosition NED
LibrePilot 5369 attitude.c Non-Control 7a3ec3e173 Some code cleanup to get rid of some warning messages float rpy 3 float q 4 gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude return 0 float lat alt
LibrePilot 5370 attitude.c Control 98b1a21921 Fixup: Shouldn t update gps until home location is set gps_updated &= gpsData.Satellites >= 7 && gpsData.PDOP <= 4.0f && homeLocation.Set == HOMELOCATION_SET_TRUE
LibrePilot 5371 attitude.c Control a83fb019f6 When in complimentary filter mode pass the GPS information straightthrough to Position Actual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsPosition.Groundspeed * cosf gpsPosition.Heading * F_PI / 180.0f velocityActual.East = gpsPosition.Groundspeed * sinf gpsPosition.Heading * F_PI / 180.0f velocityActual.Down = 0 VelocityActualSet &velocityActual
LibrePilot 5372 attitude.c Control 54880c83a0 Fix some task sizes and the memory limits #define STACK_SIZE_BYTES 1540
LibrePilot 5373 attitude.c Control 372f959c37 Refine some stack sizes #define STACK_SIZE_BYTES 2240 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE
LibrePilot 5376 attitude.c Control 319b53a342 Make sure full initialization routine for INSGPS occurs each time it isrestarted. if first_run inited = false init_stage = 0
LibrePilot 5378 attitude.c Control 3888d6ff69 Changed settings that makes updates smoother INSSetPosVelVar 1e2f 1e2f sensors |= POS_SENSORS |VERT_SENSORS
LibrePilot 5379 attitude.c Control af9f36d7df Improve the attitude initialization for all of the modes if first_run AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual MagnetometerData magData MagnetometerGet &magData float rpy 3 float q 4 rpy 0 = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI rpy 1 = atan2f accelsData.x -accelsData.z * 180.0f / F_PI rpy 2 = atan2f magData.x -magData.y * 180.0f / F_PI RPY2Quaternion rpy q quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual rpy 0 = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI rpy 1 = atan2f accelsData.x -accelsData.z * 180.0f / F_PI rpy 0 = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI rpy 1 = atan2f accelsData.x -accelsData.z * 180.0f / F_PI
LibrePilot 5382 attitude.c Control 7f226867c6 Some bug fixes for revo attitude estimation. RevoCalibrationInitialize RevoCalibrationConnectCallback &settingsUpdatedCb AccelsGet &accelsData rpy 2 = atan2f -magData.y magData.x * 180.0f / F_PI rpy 2 = atan2f -magData.y magData.x * 180.0f / F_PI rpy 2 = atan2f -magData.y magData.x * 180.0f / F_PI sensors |= POS_SENSORS | HORIZ_SENSORS
LibrePilot 5387 attitude.c Control 5b3cc4876e In simulations a higher GPS noise is required for the INS INSSetPosVelVar 1.0e0f 1.0e0f
LibrePilot 5430 attitude.c Non-Control e97c1bc016 Include the module include file before any uavo ones #include accels.h
LibrePilot 5484 attitude.c Non-Control fbd8695d7d Check in WIP for osx simulation static int i static uint32_t last_time i++ if i % 5000 == 0 float dT = PIOS_DELAY_DiffuS last_time / 10.0e6 fprintf stderr Attitude relative timing: %f\n dT last_time = PIOS_DELAY_GetRaw
LibrePilot 5497 attitude.c Non-Control 6df9691609 Bring back the original module files and remove comments from FreeRTOSdebugging
LibrePilot 5518 attitude.c Non-Control ea13536da0 Take quidance settings from simulation and make the INSGPS pick up the GPSnoise from a setting. INSSetPosVelVar revoCalibration.gps_var REVOCALIBRATION_GPS_VAR_POS revoCalibration.gps_var REVOCALIBRATION_GPS_VAR_VEL
LibrePilot 5520 attitude.c Control 345b6578b1 Fix a possible race condition for complimentary filter on L3GD20 attitudeestimation where the accel queue wouldn t immediately have data available.Added 1 ms timeout on queue. if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE
LibrePilot 5526 attitude.c Control 81fcfd45c4 Some work on the code to initialize the INSGPS and allow setting the varianceof the baro int32_t ret_val = -1 ret_val = updateAttitudeComplimentary first_run ret_val = updateAttitudeINSGPS first_run true ret_val = updateAttitudeINSGPS first_run false if ret_val == 0 first_run = false INSSetBaroVar revoCalibration.baro_var INSSetBaroVar revoCalibration.baro_var return 0 INSSetBaroVar revoCalibration.baro_var
LibrePilot 5529 attitude.c Control fcb9193d25 Disable baro updates into INSGPS for now. Seems to cause issues. sensors &= ~BARO_SENSOR sensors &= ~BARO_SENSOR NED 2 = -baroData.Altitude sensors |= VER_POS_SENSORS
LibrePilot 5530 attitude.c Control 414e62f14e Revert Disable baro updates into INSGPS for now. Seems to cause issues. This reverts commit 04591309770679e7b01c53a6cb54cac1c043f8b6. NED 2 = baroData.Altitude sensors |= POS_SENSORS |VERT_SENSORS
LibrePilot 5546 attitude.c Control 136bbe3814 Update the EKF code to use the GPSVelocity #include gpsvelocity.h static xQueueHandle gpsVelQueue gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent GPSVelocityConnectQueue gpsVelQueue GPSVelocityData gpsVelData static bool gps_vel_updated gps_vel_updated |= xQueueReceive gpsVelQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode GPSVelocityGet &gpsVelData if 0 vel 0 = gpsPosition.Groundspeed * cosf gpsPosition.Heading * F_PI / 180.0f vel 1 = gpsPosition.Groundspeed * sinf gpsPosition.Heading * F_PI / 180.0f vel 2 = 0 else vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down
LibrePilot 5563 attitude.c Control 5d160860a3 Fix from Corvus for gyro bias from EKF float gyros 3 = gyrosData.x - gyrosBias.x * F_PI / 180.0f gyrosData.y - gyrosBias.y * F_PI / 180.0f gyrosData.z - gyrosBias.z * F_PI / 180.0f gyrosBias.x = -Nav.gyro_bias 0 * 180.0f / F_PI gyrosBias.y = -Nav.gyro_bias 1 * 180.0f / F_PI gyrosBias.z = -Nav.gyro_bias 2 * 180.0f / F_PI
LibrePilot 5574 attitude.c Control 1559eeb2e1 Use GPSVelocity updates when they are available instead of timing based onGPSPosition bool first_run = true gps_vel_updated = 0 if gps_updated && outdoor_mode sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf gpsData.Heading * F_PI / 180.0f vel 1 = gpsData.Groundspeed * sinf gpsData.Heading * F_PI / 180.0f getNED &gpsData NED if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down
LibrePilot 5580 attitude.c Control a7ba6d96c1 Allow guidance to run purely off raw GPS data NEDPositionData nedPosition NEDPositionGet &nedPosition nedPosition.North = NED 0 nedPosition.East = NED 1 nedPosition.Down = NED 2 NEDPositionSet &nedPosition
LibrePilot 5626 attitude.c Control 2890fcdc9f When in complimentary filter mode use GPSVelocity so event system doesn t havewarning. if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down
LibrePilot 5627 attitude.c Control fb53d5621a Change the magKp term (hardcoded still) for revo complimentary filter so ittracks magnetic north when homelocation is set. float magKp = 0.01f magKp = 1 magKp = 1 magKp = 0.01f
LibrePilot 5650 attitude.c Control 6c0dc80978 Shuffle more stack sizes around #define STACK_SIZE_BYTES 2048
LibrePilot 5686 attitude.c Non-Control e83fec0463 Merged latest origin/next. #include gyros.h #include accels.h #include #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f static int32_t updateSensors AccelsData * GyrosData * static int32_t updateSensorsCC3D AccelsData * accelsData GyrosData * gyrosData static void updateAttitude AccelsData * GyrosData * AccelsInitialize GyrosInitialize int32_t accel_test int32_t gyro_test const struct pios_board_info * bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d #if defined PIOS_INCLUDE_MPU6000 gyro_test = PIOS_MPU6000_Test #endif else #if defined PIOS_INCLUDE_ADXL345 accel_test = PIOS_ADXL345_Test #endif #if defined PIOS_INCLUDE_ADC gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE #endif AccelsData accels GyrosData gyros int32_t retval if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 updateAttitude &accels &gyros float gyros_passed 3 static int32_t updateSensors AccelsData * accels GyrosData * gyros gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain gyros->temperature = samples_remaining accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 2 += - gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 struct pios_mpu6000_data mpu6000_data static int32_t updateSensorsCC3D AccelsData * accelsData GyrosData * gyrosData float accels 3 gyros 3 #if defined PIOS_INCLUDE_MPU6000 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 gyros 0 = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 1 = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 2 = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 1 = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 2 = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE AccelsSet &accelsData gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 GyrosSet gyrosData AccelsSet accelsData static void updateAttitude AccelsData * accelsData GyrosData * gyrosData float * gyros = &gyrosData->x float * accels = &accelsData->x float grot 3 float accel_err 3 grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 CrossProduct const float * accels const float * grot accel_err float accel_mag = sqrtf accels 0 *accels 0 + accels 1 *accels 1 + accels 2 *accels 2 if accel_mag < 1.0e-3f return accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI / 180 / 2 qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI / 180 / 2 qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI / 180 / 2 qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI / 180 / 2 float qmag = sqrtf q 0 *q 0 + q 1 *q 1 + q 2 *q 2 + q 3 *q 3
LibrePilot 5768 attitude.c Control 98655aacc8 Fix initialization of the complimentary filter where it didn t get the rightinitial dT if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / F_PI attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / F_PI RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 timeval = PIOS_DELAY_GetRaw return 0
LibrePilot 5769 attitude.c Control 2ca3cde83f Improve EKF initialization. For some reason we must read an extra mag samplewhen power up value of attitude algorithm is EKF. I don t understand why thefirst mag value is bad. static bool mag_updated = false if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / F_PI attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / F_PI RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual q 0 = attitudeActual.q1 q 1 = attitudeActual.q2 q 2 = attitudeActual.q3 q 3 = attitudeActual.q4 xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / F_PI attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / F_PI attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / F_PI RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual q 0 = attitudeActual.q1 q 1 = attitudeActual.q2 q 2 = attitudeActual.q3 q 3 = attitudeActual.q4 dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude return 0
LibrePilot 5774 attitude.c Control 836e34a483 Change revo sensors to update at 500 Hz and make sure in all cases it waits forthe relevant queue to provide data. Also disable the quaternion stabilizationflag. #if defined PIOS_INCLUDE_HMC5883 #else MagnetometerData magData magData.x = 100 magData.y = 0 magData.z = 0 #endif AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual
LibrePilot 5971 attitude.c Control 910b465ac6 AeroSimRC: do not update sensor and/or attitude data if read-only flag is set int32_t retval = 0 if cc3d if retval != 0 if !AttitudeActualReadOnly updateAttitude &accels &gyros if GyrosReadOnly || AccelsReadOnly
LibrePilot 5983 attitude.c Control f7a2b95167 Revolution/Attitude: Added offset calculation for barometric altitude #define BaroOffsetLowPassTime 300 static float baroOffset = 0 baroOffset = -home.Altitude pos 2 = - baroData.Altitude + baroOffset baroOffset = -NED 2 - baroData.Altitude if baro_updated baroOffset = 1.0f - dT/BaroOffsetLowPassTime * baroOffset + dT/BaroOffsetLowPassTime * -NED 2 - baroData.Altitude NED 2 = - baroData.Altitude + baroOffset INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors
LibrePilot 5984 attitude.c Control a786c6e6b9 Attitude: Fixed calculation for baro offset adjustment time #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f baroOffset = -baroData.Altitude baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude
LibrePilot 5987 attitude.c Control c9d41e2a23 fixes to INS backported from portugal flight branch float gyros 3 = gyrosData.x - gyrosBias.x * F_PI / 180.0f gyrosData.y - gyrosBias.y * F_PI / 180.0f gyrosData.z - gyrosBias.z * F_PI / 180.0f if fabs Nav.gyro_bias 0 > 0.5f || fabs Nav.gyro_bias 1 > 0.5f || fabs Nav.gyro_bias 2 > 0.5f
LibrePilot 6004 attitude.c Control b06b51f1b2 Revolution/Attitude: Added offset calculation for barometric altitude #define BaroOffsetLowPassTime 300 static float baroOffset = 0 baroOffset = -home.Altitude pos 2 = - baroData.Altitude + baroOffset baroOffset = -NED 2 - baroData.Altitude if baro_updated baroOffset = 1.0f - dT/BaroOffsetLowPassTime * baroOffset + dT/BaroOffsetLowPassTime * -NED 2 - baroData.Altitude NED 2 = - baroData.Altitude + baroOffset INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors
LibrePilot 6005 attitude.c Control be72d24c5f Attitude: Fixed calculation for baro offset adjustment time #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f baroOffset = -baroData.Altitude baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude
LibrePilot 6075 attitude.c Non-Control 87900043a4 Typo fix. Grammarians will be extreamely glad to complement the OP team now that these errors haved disappeared. static int32_t updateAttitudeComplementary bool first_run case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run static int32_t updateAttitudeComplementary bool first_run
LibrePilot 6189 attitude.c Control 797a4def6a Do not use the data from the magnetometer if it contains NAN if mag.x == mag.x && mag.y == mag.y && mag.z == mag.z rot_mult Rbe home.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1 || mag_len < 1 mag_err 0 = mag_err 1 = mag_err 2 = 0 else CrossProduct const float * &mag.x const float * brot mag_err mag_updated &= magData.x == magData.x && magData.y == magData.y && magData.z == magData.z
LibrePilot 6192 attitude.c Control e5bd999975 Process the settings updates more discretely in revo attitude static bool gyroBiasSettingsUpdated = false settingsUpdatedCb RevoCalibrationHandle settingsUpdatedCb HomeLocationHandle static void settingsUpdatedCb UAVObjEvent * ev if ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = revoCalibration.gyro_bias REVOCALIBRATION_GYRO_BIAS_X gyrosBias.y = revoCalibration.gyro_bias REVOCALIBRATION_GYRO_BIAS_X gyrosBias.z = revoCalibration.gyro_bias REVOCALIBRATION_GYRO_BIAS_X GyrosBiasSet &gyrosBias gyroBiasSettingsUpdated = true INSSetMagVar revoCalibration.mag_var INSSetAccelVar revoCalibration.accel_var INSSetGyroVar revoCalibration.gyro_var INSSetBaroVar revoCalibration.baro_var else if ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = homeLocation.Latitude / 10.0e6f * DEG2RAD alt = homeLocation.Altitude T 0 = alt+6.378137E6f T 1 = cosf lat * alt+6.378137E6f T 2 = -1.0f else if ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings else if ev->obj == RevoSettingsHandle RevoSettingsGet &revoSettings
LibrePilot 6193 attitude.c Control 7cdf47c1d4 Make the sensors code only apply the GyroBias from the UAVO which is now comingconsistently only from the attitude module rot_mult Rbe homeLocation.Be brot GyrosBiasGet &gyrosBias INSSetGyroBias &gyrosBias.x INSSetGyroBias &gyrosBias.x if gyroBiasSettingsUpdated INSSetGyroBias &gyrosBias.x gyroBiasSettingsUpdated = false float gyros 3 = gyrosData.x * F_PI / 180.0f gyrosData.y * F_PI / 180.0f gyrosData.z * F_PI / 180.0f if revoCalibration.BiasCorrectedRaw gyros 0 -= gyrosBias.x * F_PI / 180.0f gyros 1 -= gyrosBias.y * F_PI / 180.0f gyros 2 -= gyrosBias.z * F_PI / 180.0f if revoCalibration.BiasCorrectedRaw && !gyroBiasSettingsUpdated gyrosBias.x = -Nav.gyro_bias 0 * 180.0f / F_PI gyrosBias.y = -Nav.gyro_bias 1 * 180.0f / F_PI gyrosBias.z = -Nav.gyro_bias 2 * 180.0f / F_PI GyrosBiasSet &gyrosBias
LibrePilot 6195 attitude.c Non-Control 69057a1373 No reason to get homeLocation every cycle INSSetMagNorth homeLocation.Be INSSetMagNorth homeLocation.Be
LibrePilot 6196 attitude.c Non-Control 3f4706ad4c Make it a critical error when an invalid attitude algorithm is selected AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL
LibrePilot 6197 attitude.c Control 02dfa7bd82 Change how the updated settings in attitude are changed to make it easier toinitialize them all settingsUpdatedCb NULL if ev == NULL || ev->obj == RevoCalibrationHandle gyrosBias.y = revoCalibration.gyro_bias REVOCALIBRATION_GYRO_BIAS_Y gyrosBias.z = revoCalibration.gyro_bias REVOCALIBRATION_GYRO_BIAS_Z if ev == NULL || ev->obj == HomeLocationHandle if ev == NULL || ev->obj == AttitudeSettingsHandle if ev == NULL || ev->obj == RevoSettingsHandle
LibrePilot 6199 attitude.c Control 3e33bb49a2 EKF gyro bias into the object now working propely float gyro_bias 3 = -gyrosBias.x * F_PI / 180.0f -gyrosBias.y * F_PI / 180.0f -gyrosBias.z * F_PI / 180.0f INSSetGyroBias gyro_bias float gyro_bias 3 = -gyrosBias.x * F_PI / 180.0f -gyrosBias.y * F_PI / 180.0f -gyrosBias.z * F_PI / 180.0f INSSetGyroBias gyro_bias float gyro_bias 3 = -gyrosBias.x * F_PI / 180.0f -gyrosBias.y * F_PI / 180.0f -gyrosBias.z * F_PI / 180.0f INSSetGyroBias gyro_bias if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated gyrosBias.x = -Nav.gyro_bias 0 * 180.0f / F_PI gyrosBias.y = -Nav.gyro_bias 1 * 180.0f / F_PI gyrosBias.z = -Nav.gyro_bias 2 * 180.0f / F_PI GyrosBiasSet &gyrosBias
LibrePilot 6200 attitude.c Control d36663dbbd Treat the GyroBias UAVO like a state estimate of the actual gyro bias so now wesubtract that from the raw sensor readings to get the Gyros UAVO value gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi float gyro_bias 3 = gyrosBias.x * F_PI / 180.0f gyrosBias.y * F_PI / 180.0f gyrosBias.z * F_PI / 180.0f float gyro_bias 3 = gyrosBias.x * F_PI / 180.0f gyrosBias.y * F_PI / 180.0f gyrosBias.z * F_PI / 180.0f float gyro_bias 3 = gyrosBias.x * F_PI / 180.0f gyrosBias.y * F_PI / 180.0f gyrosBias.z * F_PI / 180.0f gyros 0 += gyrosBias.x * F_PI / 180.0f gyros 1 += gyrosBias.y * F_PI / 180.0f gyros 2 += gyrosBias.z * F_PI / 180.0f gyrosBias.x = Nav.gyro_bias 0 * 180.0f / F_PI gyrosBias.y = Nav.gyro_bias 1 * 180.0f / F_PI gyrosBias.z = Nav.gyro_bias 2 * 180.0f / F_PI
LibrePilot 6202 attitude.c Control fa68c0422d Make sure to always compare BiasCorrectedRaw against its enum. Also reversedthe order of TRUE FALSE in the definition to make it safer anyway. if revoCalibration.BiasCorrectedRaw = REVOCALIBRATION_BIASCORRECTEDRAW_TRUE
LibrePilot 6204 attitude.c Non-Control 5649813c2b Make sure to always compare BiasCorrectedRaw against its enum. Also reversedthe order of TRUE FALSE in the definition to make it safer anyway. if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE
LibrePilot 6207 attitude.c Control 9c512261e6 When in indoor mode reset the baro offset to zero baroOffset = 0
LibrePilot 6211 attitude.c Control f3f34e8f9f AeroSimRC: fix CC3D virtual sensor readings in simulation mode if GyrosReadOnly || AccelsReadOnly return 0
LibrePilot 6212 attitude.c Control 4e42fb564e AeroSimRC: fix CC3D virtual sensor readings in simulation mode if GyrosReadOnly || AccelsReadOnly return 0
LibrePilot 6230 attitude.c Non-Control a77a012586 Eliminated attitude sensor timeout warnings when in simulation mode. xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1
LibrePilot 6335 attitude.c Control 3d1a6cbcba Attitude: Make sure the EKF can init if HomeLocation is not set but themagnetic field is. Good for switching between indoor and outdoor mode. mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode gps_vel_updated |= xQueueReceive gpsVelQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode mag_updated &= homeLocation.Be 0 != 0 || homeLocation.Be 1 != 0 || homeLocation.Be 2
LibrePilot 6431 attitude.c Control 4803dfe99c Fix: yaw bias correction wasn t applied on CC3D. gyro_correct_int 2 += - gyrosData->z * yawBiasRate
LibrePilot 6446 attitude.c Control 88b483f37d PIOS_MPU6000: Make the driver perform the rotation to bring it into the OPcoordinate systemPreviously there were hacks spread throughout to deal with various ways ofpositioning the chips. Now the driver structure specifies the rotationof the chip relative to the board layout and the output X/Y/Z are alreadyin OP convention.This flag seems to do the right thing for Revolution CC3D and RevoMini gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale
LibrePilot 6598 attitude.c Control c207afbfef Implement smoothing filter for accelerometer data. static float accel_alpha = 0 static float accels_filtered 3 accels_filtered 0 = accels_filtered 0 * accel_alpha + accels 0 * 1 - accel_alpha accels_filtered 1 = accels_filtered 1 * accel_alpha + accels 1 * 1 - accel_alpha accels_filtered 2 = accels_filtered 2 * accel_alpha + accels 2 * 1 - accel_alpha CrossProduct const float * accels_filtered const float * grot accel_err float accel_mag = sqrtf accels_filtered 0 *accels_filtered 0 + accels_filtered 1 *accels_filtered 1 + accels_filtered 2 *accels_filtered 2 const float fakeDt = 0.0025 if attitudeSettings.AccelTau < 0.0001 accel_alpha = 0 else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau
LibrePilot 6600 attitude.c Non-Control 10865221b9 Removed a spurious call to AccelsSet() that was causing garbage data to be transiently written to the Accels UAVO. Props to cyr for kicking off the discussion that led to the discovery of this bug.
LibrePilot 6601 attitude.c Control b7bdf9861d Run rotated gravity vector through smoothing filter to match phase with filtered accelerometer data. static float grot 3 grot 0 = grot 0 * accel_alpha - 2 * q 1 * q 3 - q 0 * q 2 * 1 - accel_alpha grot 1 = grot 1 * accel_alpha - 2 * q 2 * q 3 + q 0 * q 1 * 1 - accel_alpha grot 2 = grot 2 * accel_alpha - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 * 1 - accel_alpha
LibrePilot 6602 attitude.c Control f63db712d0 Disable accel smoothing during init/arming so it does not interfere static float accel_alpha_setting = 0 accel_alpha = 0 accel_alpha = 0 accel_alpha = accel_alpha_setting accel_alpha_setting = 0 accel_alpha_setting = expf -fakeDt / attitudeSettings.AccelTau accel_alpha = accel_alpha_setting
LibrePilot 6603 attitude.c Control 677f921b15 Take magnitude of filtered gravity vector into account. float grot_mag = sqrtf grot 0 *grot 0 + grot 1 *grot 1 + grot 2 *grot 2 if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag*grot_mag accel_err 1 /= accel_mag*grot_mag accel_err 2 /= accel_mag*grot_mag
LibrePilot 6617 attitude.c Control 3c8ffa9773 Revert When in indoor mode reset the baro offset to zero This reverts commit 9c512261e67deb39dc493579b9f7db890f54b9ed.
LibrePilot 6618 attitude.c Non-Control 5981d9acd6 removed surplus NEDposition uavobject
LibrePilot 6621 attitude.c Control 104c61a174 Move accel filter to separate function for cleaner code and easier filter alteration.Optimize for filter disabled case (AccelTau = 0.0). static bool accel_filter_enabled = false static float accels_filtered 3 static float grot_filtered 3 accel_filter_enabled = false accel_filter_enabled = false if accel_alpha > 0.0f accel_filter_enabled = true static inline void apply_accel_filter const float * raw float * filtered if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 float grot 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 *q 1 - q 2 *q 2 + q 3 *q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float grot_mag if accel_filter_enabled grot_mag = sqrtf grot 0 *grot 0 + grot 1 *grot 1 + grot 2 *grot 2 else grot_mag = 1.0f if attitudeSettings.AccelTau < 0.0001 accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true
LibrePilot 6629 attitude.c Control 064ed4087c set AirspeedActual from attitude module feed true airspeed estimation into EKF to allow more accurate INS #include airspeedsensor.h #include airspeedactual.h #define IAS2TAS alt 1.0f + 0.02f* alt /304.8f static xQueueHandle airspeedQueue AirspeedActualInitialize AirspeedSensorInitialize airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent AirspeedSensorConnectQueue airspeedQueue if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed AirspeedSensorData airspeedData static bool airspeed_updated airspeed_updated = 0 airspeed_updated = 0 airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE AirspeedSensorGet &airspeedData airspeed_updated &= airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE INSSetPosVelVar 1e6f 1e5f if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar 1e6f 1e2f float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0 0 rot_mult R vtas vel
LibrePilot 6631 attitude.c Non-Control 54ebcb1ea8 Removed a spurious call to AccelsSet() that was causing garbage data to be transiently written to the Accels UAVO. Props to cyr for kicking off the discussion that led to the discovery of this bug.
LibrePilot 6636 attitude.c Non-Control 04c194fa48 Coding style fixes if accel_alpha > 0.0f static inline void apply_accel_filter const float *raw float *filtered if accel_filter_enabled apply_accel_filter accels accels_filtered apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err if accel_mag < 1.0e-3f if accel_filter_enabled grot_mag = sqrtf grot 0 *grot 0 + grot 1 *grot 1 + grot 2 *grot 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f if attitudeSettings.AccelTau < 0.0001
LibrePilot 6637 attitude.c Non-Control e38ba912cd Clarify comment about gravity vector
LibrePilot 6638 attitude.c Control f9f58f22d8 Take the magnitude of the correct gravity vector grot_mag = sqrtf grot_filtered 0 *grot_filtered 0 + grot_filtered 1 *grot_filtered 1 + grot_filtered 2 *grot_filtered 2
LibrePilot 6660 attitude.c Control 5132448fe8 Decrease zeroing yawBiasRate to reduce noise-induced random yaw drift. yawBiasRate = 0.01 yawBiasRate = 0.01
LibrePilot 7774 attitude.c Control ce6f84063c OP-908 Cleanup for unwanted double precision math on float data.- Use float counterpart instead of double precision function when dealing with float data- Force float data type for decimal constants+review OPReview qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * float M_PI / 180 / 2 qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * float M_PI / 180 / 2 qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * float M_PI / 180 / 2 qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * float M_PI / 180 / 2 if fabsf qmag < 1e-3f || qmag != qmag if attitudeSettings.AccelTau < 0.0001f
LibrePilot 7784 attitude.c Control 71de38f2d5 OP-908 Uniform handling of M_PI using a Floating counterpart F_PI explicitly define integer constants as float when used in floating point calculations+review OPReview-436 #define F_PI float M_PI qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * F_PI / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * F_PI / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * F_PI / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * F_PI / 180.0f / 2.0f
LibrePilot 8181 attitude.c Control cc6d19e40a OP-908 added pios_math.h and pios_helper.h to contains all the various math and general macros/constants. Fixed remaining constants still defined as double.+review OPReview-436 #include #define PI_MOD x fmodf x + M_PI_F M_PI_F * 2 - M_PI_F attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / M_PI_F attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / M_PI_F attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / M_PI_F qdot 0 = -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT * M_PI_F / 180 / 2 qdot 1 = q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT * M_PI_F / 180 / 2 qdot 2 = q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT * M_PI_F / 180 / 2 qdot 3 = -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT * M_PI_F / 180 / 2 float gyro_bias 3 = gyrosBias.x * M_PI_F / 180.0f gyrosBias.y * M_PI_F / 180.0f gyrosBias.z * M_PI_F / 180.0f attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / M_PI_F attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / M_PI_F attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / M_PI_F float gyro_bias 3 = gyrosBias.x * M_PI_F / 180.0f gyrosBias.y * M_PI_F / 180.0f gyrosBias.z * M_PI_F / 180.0f attitudeActual.Roll = atan2f -accelsData.y -accelsData.z * 180.0f / M_PI_F attitudeActual.Pitch = atan2f accelsData.x -accelsData.z * 180.0f / M_PI_F attitudeActual.Yaw = atan2f -magData.y magData.x * 180.0f / M_PI_F float gyros 3 = gyrosData.x + gyrosBias.x * M_PI_F / 180.0f gyrosData.y + gyrosBias.y * M_PI_F / 180.0f gyrosData.z + gyrosBias.z * M_PI_F / 180.0f float gyro_bias 3 = gyrosBias.x * M_PI_F / 180.0f gyrosBias.y * M_PI_F / 180.0f gyrosBias.z * M_PI_F / 180.0f float gyros 3 = gyrosData.x * M_PI_F / 180.0f gyrosData.y * M_PI_F / 180.0f gyrosData.z * M_PI_F / 180.0f gyros 0 += gyrosBias.x * M_PI_F / 180.0f gyros 1 += gyrosBias.y * M_PI_F / 180.0f gyros 2 += gyrosBias.z * M_PI_F / 180.0f vel 0 = gpsData.Groundspeed * cosf gpsData.Heading * M_PI_F / 180.0f vel 1 = gpsData.Groundspeed * sinf gpsData.Heading * M_PI_F / 180.0f gyrosBias.x = Nav.gyro_bias 0 * 180.0f / M_PI_F gyrosBias.y = Nav.gyro_bias 1 * 180.0f / M_PI_F gyrosBias.z = Nav.gyro_bias 2 * 180.0f / M_PI_F
LibrePilot 8229 attitude.c Control 36c0ec100b OP-908 added pios_constants.h containing some (more or less) useful physical constants added other float constants to pios_math.h fixes for code style.+review OPReview-436 #include if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f
LibrePilot 8230 attitude.c Control 36c0ec100b OP-908 added pios_constants.h containing some (more or less) useful physical constants added other float constants to pios_math.h fixes for code style.+review OPReview-436 #include if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f
LibrePilot 8965 attitude.c Non-Control 66db978244 Rename Libraries->libraries Modules->modules
LibrePilot 8966 attitude.c Non-Control 66db978244 Rename Libraries->libraries Modules->modules
LibrePilot 9936 attitude.c Non-Control 5f3e0c3e1d Math cleanup attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude lat = DEG2RAD homeLocation.Latitude / 10.0e6f
LibrePilot 9941 attitude.c Control e3b3713139 HiTL simulation solution for GPS update if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode if !GPSVelocityReadOnly gps_vel_updated |= xQueueReceive gpsVelQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_vel_updated |= pdTRUE && outdoor_mode
LibrePilot 9947 attitude.c Non-Control 80c917591e Cherry-pick and hand-merge of Sami s math cleanup commit from branch sambas/diffnext: 5f3e0c3e1da9388ca7572a5cb97d67a002dc0360 attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x attitudeActual.Roll = RAD2DEG atan2f -accelsData.y -accelsData.z attitudeActual.Pitch = RAD2DEG atan2f accelsData.x -accelsData.z attitudeActual.Yaw = RAD2DEG atan2f -magData.y magData.x vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude lat = DEG2RAD homeLocation.Latitude / 10.0e6f
LibrePilot 9956 attitude.c Control dff6c2cb98 handle GyroBias more gracefully:Its now only a dynamic offset to a static calibration. Also have to complementary filter still work with uncorrected raw values. if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z float gyro_bias 3 = 0 0 0 float gyro_bias 3 = 0 0 0 float gyros 3 = gyrosData.x * M_PI_F / 180.0f gyrosData.y * M_PI_F / 180.0f gyrosData.z * M_PI_F / 180.0f if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += gyrosBias.x * M_PI_F / 180.0f gyros 1 += gyrosBias.y * M_PI_F / 180.0f gyros 2 += gyrosBias.z * M_PI_F / 180.0f gyrosBias.x = Nav.gyro_bias 0 * 180.0f / M_PI_F gyrosBias.y = Nav.gyro_bias 1 * 180.0f / M_PI_F gyrosBias.z = Nav.gyro_bias 2 * 180.0f / M_PI_F GyrosBiasSet &gyrosBias
LibrePilot 9958 attitude.c Control 63c4dc1ff0 Expose gyro bias process variance to settings UAVObject code cleanup in revolution/attitude to make it more readable dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if init_stage == 0 float Pdiag 13 = 25.0f 25.0f 25.0f 4.0f 4.0f 4.0f 2e-6f 2e-6f 2e-6f 2e-6f 8e-3f 8e-3f 8e-3f INSSetGyroBiasVar revoCalibration.gyro_bias_var if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset INSSetState pos zeros q zeros zeros else INSSetGyroBiasVar revoCalibration.gyro_bias_var
LibrePilot 9959 attitude.c Control 14f72459c6 Changed code for attitude initialisation so it works when the board is upside down attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cos attitudeActual.Roll * magData.z + sin attitudeActual.Roll * magData.y float yn = cos attitudeActual.Roll * magData.y - sin attitudeActual.Roll * magData.z float azn = cos attitudeActual.Roll * accelsData.z + sin attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cos attitudeActual.Pitch * magData.x + sin attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll *= 180.0f / M_PI_F attitudeActual.Pitch *= 180.0f / M_PI_F attitudeActual.Yaw *= 180.0f / M_PI_F AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cos attitudeActual.Roll * magData.z + sin attitudeActual.Roll * magData.y float yn = cos attitudeActual.Roll * magData.y - sin attitudeActual.Roll * magData.z float azn = cos attitudeActual.Roll * accelsData.z + sin attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cos attitudeActual.Pitch * magData.x + sin attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll *= 180.0f / M_PI_F attitudeActual.Pitch *= 180.0f / M_PI_F attitudeActual.Yaw *= 180.0f / M_PI_F
LibrePilot 9961 attitude.c Control 6e2c9b3c46 New EKFStateVariance UAVObject with EKF state variance information #include ekfstatevariance.h EKFStateVarianceInitialize EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata
LibrePilot 9962 attitude.c Control c06f049e2a adjusted initial variances and defaults to be closer to the original values to reduce possibly negative impacts of the previous changesets 5.0f 5.0f 5.0f 7e-3f 7e-3f 7e-3f 7e-3f 1e-4f 1e-4f 1e-4f
LibrePilot 9963 attitude.c Control 0536936d45 Safety checks for NAN and INF and invalid variance configuration to protect EKF from harm static bool volatile variance_error = true static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if fabsf data < 1e-15f return true return false static bool value_error = false value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0 gyrosBias.y = 0 gyrosBias.z = 0 if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 ==0 && homeLocation.Be 1 ==0 && homeLocation.Be 2 ==0 mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var revoCalibration.gps_var REVOCALIBRATION_GPS_VAR_POS || invalid_var revoCalibration.gps_var REVOCALIBRATION_GPS_VAR_VEL gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude==0 && gpsData.Longitude==0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 else if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error bool error = false if invalid_var revoCalibration.mag_var 0 || invalid_var revoCalibration.mag_var 1 || invalid_var revoCalibration.mag_var 2 || invalid_var revoCalibration.accel_var 0 || invalid_var revoCalibration.accel_var 1 || invalid_var revoCalibration.accel_var 2 || invalid_var revoCalibration.gyro_var 0 || invalid_var revoCalibration.gyro_var 1 || invalid_var revoCalibration.gyro_var 2 || invalid_var revoCalibration.gyro_bias_var 0 || invalid_var revoCalibration.gyro_bias_var 1 || invalid_var revoCalibration.gyro_bias_var 2 || invalid_var revoCalibration.baro_var error = true if error variance_error = true else INSSetMagVar revoCalibration.mag_var INSSetAccelVar revoCalibration.accel_var INSSetGyroVar revoCalibration.gyro_var INSSetGyroBiasVar revoCalibration.gyro_bias_var INSSetBaroVar revoCalibration.baro_var variance_error = false
LibrePilot 9970 attitude.c Control 7937ae6296 OP-931: Makes flight code compile with -Wfloat-equal and -Wunsuffixed-float-constants enabled.Also fixes warnings (and bugs) in F4 STM32_USB_OTG_Driver code allowing -Werror to be enabled for all flight code.Fixes all other compiler warnings that would otherwise cause the flight code to not compile with -Werror enabled.Along the way this also adds some uses of isnan() to various places rather than questionable tests for x != x andx == x to check for NaNs.+review OPReview static float gyroGain = 0.42f accelKp = 1.0f accelKi = 0.9f yawBiasRate = 0.01f accelKp = 1.0f accelKi = 0.9f yawBiasRate = 0.01f dT = thisSysTime == lastSysTime ? 0.001f : portMAX_DELAY & thisSysTime - lastSysTime / portTICK_RATE_MS / 1000.0f if fabsf qmag < 1e-3f || isnan qmag else q 0 = q 0 /qmag q 1 = q 1 /qmag q 2 = q 2 /qmag q 3 = q 3 /qmag const float fakeDt = 0.0025f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z if fabs qmag < 1.0e-3f || isnan qmag mag_updated &= !isnan magData.x && !isinf magData.x && !isnan magData.y && !isinf magData.y && !isnan magData.z && !isinf magData.z mag_updated &= homeLocation.Be 0 > 0.0f || homeLocation.Be 1 > 0.0f || homeLocation.Be 2 > 0.0f
LibrePilot 9971 attitude.c Control 7937ae6296 OP-931: Makes flight code compile with -Wfloat-equal and -Wunsuffixed-float-constants enabled.Also fixes warnings (and bugs) in F4 STM32_USB_OTG_Driver code allowing -Werror to be enabled for all flight code.Fixes all other compiler warnings that would otherwise cause the flight code to not compile with -Werror enabled.Along the way this also adds some uses of isnan() to various places rather than questionable tests for x != x andx == x to check for NaNs.+review OPReview static float gyroGain = 0.42f accelKp = 1.0f accelKi = 0.9f yawBiasRate = 0.01f accelKp = 1.0f accelKi = 0.9f yawBiasRate = 0.01f dT = thisSysTime == lastSysTime ? 0.001f : portMAX_DELAY & thisSysTime - lastSysTime / portTICK_RATE_MS / 1000.0f if fabsf qmag < 1e-3f || isnan qmag else q 0 = q 0 /qmag q 1 = q 1 /qmag q 2 = q 2 /qmag q 3 = q 3 /qmag const float fakeDt = 0.0025f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z if fabs qmag < 1.0e-3f || isnan qmag mag_updated &= !isnan magData.x && !isinf magData.x && !isnan magData.y && !isinf magData.y && !isnan magData.z && !isinf magData.z mag_updated &= homeLocation.Be 0 > 0.0f || homeLocation.Be 1 > 0.0f || homeLocation.Be 2 > 0.0f
LibrePilot 10014 attitude.c Non-Control 3ef2693dfc use DEG2RAD and RAD2DEG everywhere qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2
LibrePilot 10015 attitude.c Control e7dc665c14 Reworked Variance Objects Setting for initial EKF variance #include ekfinitialvariance.h EKFInitialVarianceInitialize float pos 3 = 0.0f 0.0f 0.0f float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 EKFInitialVarianceData vardata EKFInitialVarianceGet &vardata INSResetP vardata.P
LibrePilot 10016 attitude.c Control ff5fd9c7c9 Safety modification defer any change to RevoSettings (fusionalgorithm) RevoCalibration and EKFInitialVariance until the craft has been disarmed! static EKFInitialVarianceData ekfInitialVariance static FlightStatusData flightStatus static bool volatile initialization_required = true static uint32_t volatile running_algorithm = 0xffffffff FlightStatusInitialize EKFInitialVarianceConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb bool first_run = false if initialization_required initialization_required = false switch running_algorithm if ret_val != 0 initialization_required = true if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 INSResetP ekfInitialVariance.P if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle || ev->obj == EKFInitialVarianceHandle || ev->obj == RevoSettingsHandle || variance_error==true && ev->obj == FlightStatusHandle EKFInitialVarianceGet &ekfInitialVariance int t for t=0 t < EKFINITIALVARIANCE_P_NUMELEM t++ if invalid_var ekfInitialVariance.P t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true running_algorithm = revoSettings.FusionAlgorithm initialization_required = true
LibrePilot 10017 attitude.c Control 467b22f294 I knew I had forgotten something (cos -> cosf ...) float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn
LibrePilot 10022 attitude.c Control 201eed8d8f when testing variances for validity dont allow negative variances if data < 1e-15f
LibrePilot 10023 attitude.c Control 48288c740e OP-931: some floating point math fixes should make merge with other dev branch easier attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f if q 0 < 0.0f q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f static float baroOffset = 0.0f gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if fabsf homeLocation.Be 0 <0.1f && fabsf homeLocation.Be 1 <0.1f && fabsf homeLocation.Be 2 <0.1f float gyro_bias 3 = 0.0f 0.0f 0.0f vel 2 = 0.0f vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f
LibrePilot 10031 attitude.c Non-Control 84e1a81f8b OP-936: Moves the task monitor code out of the flight library and into PiOS.This move and rework also breaks the dependency of the task monitor on the UAVO subsystem and pushes the responsibility for updating the TaskInfo UAVO into the System module.+review OPReview #include #include #include taskinfo.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle #include #include taskinfo.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle
LibrePilot 10032 attitude.c Non-Control 84e1a81f8b OP-936: Moves the task monitor code out of the flight library and into PiOS.This move and rework also breaks the dependency of the task monitor on the UAVO subsystem and pushes the responsibility for updating the TaskInfo UAVO into the System module.+review OPReview #include #include #include taskinfo.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle #include #include taskinfo.h PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle
LibrePilot 10068 attitude.c Control 25c58ff074 Put all EKF initialisation values in a separate UAVObject EKFConfiguration adapted insgps13state.c/.h modules/Attitude/revolution configrevowidget.cpp accordingly. #include ekfconfiguration.h static EKFConfigurationData ekfConfiguration EKFConfigurationInitialize EKFConfigurationConnectCallback &settingsUpdatedCb if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN INSSetMagVar &ekfConfiguration.R EKFCONFIGURATION_R_MAGX INSSetAccelVar &ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX INSSetGyroVar &ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX INSSetGyroBiasVar &ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ INSResetP ekfConfiguration.P INSSetPosVelVar &ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH &ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH INSSetPosVelVar float 3 1e6f 1e6f 1e6f float 3 1e5f 1e5f 1e5f INSSetPosVelVar float 3 1e6f 1e6f 1e6f float 3 1e2f 1e2f 1e2f if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration ev->obj == EKFConfigurationHandle || EKFConfigurationGet &ekfConfiguration for t=0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t=0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t=0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t
LibrePilot 10069 attitude.c Non-Control 5d5a3389c4 missed removal of unneeded update chec if ev == NULL ||
LibrePilot 10071 attitude.c Control 0178d666d5 OP-931: Fixes the mag_updated test in revo Attitude module to use the square of the Be vector magnitude.Also fixes the dT calculation issues in a few places one of which was as suggested in OPReview-459.+review OPReview-459 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f mag_updated &= homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 > 1e-5f
LibrePilot 10072 attitude.c Control 0178d666d5 OP-931: Fixes the mag_updated test in revo Attitude module to use the square of the Be vector magnitude.Also fixes the dT calculation issues in a few places one of which was as suggested in OPReview-459.+review OPReview-459 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f mag_updated &= homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 > 1e-5f
LibrePilot 10083 attitude.c Control 576d33464a OP-754/OPReview-374: Applied a very old patch provided by cyr to the latest next branch. static float rollPitchBiasRate = 0.0f accelKi = 0.0f rollPitchBiasRate = 0.01f accelKi = 0.0f rollPitchBiasRate = 0.01f rollPitchBiasRate = 0.0f gyro_correct_int 0 += - gyros->x * rollPitchBiasRate gyro_correct_int 1 += - gyros->y * rollPitchBiasRate gyro_correct_int 0 += - gyros->x * rollPitchBiasRate gyro_correct_int 1 += - gyros->y * rollPitchBiasRate
LibrePilot 10084 attitude.c Non-Control 7f273bd9e6 Fixes recent but broken commit of fix for OP-754. Bunny is bad. rollPitchBiasRate = 0.0f gyro_correct_int 0 += - gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += - gyrosData->y * rollPitchBiasRate
LibrePilot 10086 attitude.c Control 5b61466259 OP-931: Adds -Wdouble-promotion compiler option and fixes problems that the compiler then identifies.+review OPReview-459 if fabsf qmag < 1.0e-3f || isnan qmag
LibrePilot 10104 attitude.c Non-Control a2d8544931 OP-931: adds -Wextra compiler option for the flight code and makes the bazillion code changes requiredto make the flight code compile again. Needs careful review particularly all the fixes for thesigned vs unsigned comparisons.+review OPReview-459 static void AttitudeTask __attribute__ unused void *parameters static void settingsUpdatedCb __attribute__ unused UAVObjEvent * objEv static void AttitudeTask __attribute__ unused void *parameters
LibrePilot 10105 attitude.c Non-Control a2d8544931 OP-931: adds -Wextra compiler option for the flight code and makes the bazillion code changes requiredto make the flight code compile again. Needs careful review particularly all the fixes for thesigned vs unsigned comparisons.+review OPReview-459 static void AttitudeTask __attribute__ unused void *parameters static void settingsUpdatedCb __attribute__ unused UAVObjEvent * objEv static void AttitudeTask __attribute__ unused void *parameters
LibrePilot 10195 attitude.c Control e0a6dae46a made fake variances set in indoor mode and used for airspeed hack come from UAvObject INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED
LibrePilot 10977 attitude.c Non-Control 80caceb984 make uncrustify_all;make uncrustify_all; #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 10978 attitude.c Non-Control 80caceb984 make uncrustify_all;make uncrustify_all; #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 11209 attitude.c Non-Control 5284195c29 Refaktored sensor and state UAVObjects consistently into XXYSensor and XXYState * Specifically updates the the @ref AttitudeState AttitudeState and @ref AttitudeRaw AttitudeRaw settings objects * Output objects: @ref AttitudeRaw @ref AttitudeState #include gyrostate.h #include accelstate.h #include attitudestate.h static int32_t updateSensors AccelStateData * GyroStateData * static int32_t updateSensorsCC3D AccelStateData *accelStateData GyroStateData *gyrosData static void updateAttitude AccelStateData * GyroStateData * AttitudeStateInitialize AccelStateInitialize GyroStateInitialize AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude AccelStateData accelState GyroStateData gyros retval = updateSensorsCC3D &accelState &gyros retval = updateSensors &accelState &gyros if !AttitudeStateReadOnly updateAttitude &accelState &gyros static int32_t updateSensors AccelStateData *accelState GyroStateData *gyros if GyroStateReadOnly || AccelStateReadOnly accelState->x = vec_out 0 accelState->y = vec_out 1 accelState->z = vec_out 2 gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 accelState->x = accel 0 accelState->y = accel 1 accelState->z = accel 2 trim_accels 0 += accelState->x trim_accels 1 += accelState->y trim_accels 2 += accelState->z accelState->x = accelState->x - accelbias 0 * ACCEL_SCALE accelState->y = accelState->y - accelbias 1 * ACCEL_SCALE accelState->z = accelState->z - accelbias 2 * ACCEL_SCALE GyroStateSet gyros AccelStateSet accelState static int32_t updateSensorsCC3D AccelStateData *accelStateData GyroStateData *gyrosData if GyroStateReadOnly || AccelStateReadOnly accelStateData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelStateData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelStateData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 GyroStateSet gyrosData AccelStateSet accelStateData static void updateAttitude AccelStateData *accelStateData GyroStateData *gyrosData float *accels = &accelStateData->x AttitudeStateData attitudeState AttitudeStateGet &attitudeState quat_copy q &attitudeState.q1 Quaternion2RPY &attitudeState.q1 &attitudeState.Roll AttitudeStateSet &attitudeState * Specifically updates the the @ref AttitudeState AttitudeState and @ref AttitudeRaw AttitudeRaw settings objects * Output objects: @ref AttitudeRaw @ref AttitudeState #include accelsensor.h #include accelstate.h #include airspeedstate.h #include attitudestate.h #include barosensor.h #include gyrostate.h #include gyrosensor.h #include magnetosensor.h #include magnetostate.h #include positionstate.h #include velocitystate.h AttitudeStateInitialize AirspeedStateInitialize PositionStateInitialize VelocityStateInitialize AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude GyroSensorConnectQueue gyroQueue AccelSensorConnectQueue accelQueue MagnetoSensorConnectQueue magQueue BaroSensorConnectQueue baroQueue GyroSensorData gyroSensorData AccelSensorData accelSensorData if !AttitudeStateReadOnly AccelSensorGet &accelSensorData MagnetoSensorData magData MagnetoSensorGet &magData MagnetoSensorData magData AttitudeStateData attitudeState AttitudeStateGet &attitudeState attitudeState.Roll = atan2f -accelSensorData.y -accelSensorData.z float zn = cosf attitudeState.Roll * magData.z + sinf attitudeState.Roll * magData.y float yn = cosf attitudeState.Roll * magData.y - sinf attitudeState.Roll * magData.z float azn = cosf attitudeState.Roll * accelSensorData.z + sinf attitudeState.Roll * accelSensorData.y attitudeState.Pitch = atan2f accelSensorData.x -azn float xn = cosf attitudeState.Pitch * magData.x + sinf attitudeState.Pitch * zn attitudeState.Yaw = atan2f -yn xn attitudeState.Roll = RAD2DEG attitudeState.Roll attitudeState.Pitch = RAD2DEG attitudeState.Pitch attitudeState.Yaw = RAD2DEG attitudeState.Yaw RPY2Quaternion &attitudeState.Roll &attitudeState.q1 AttitudeStateSet &attitudeState GyroSensorGet &gyroSensorData AttitudeStateData attitudeState AttitudeStateGet &attitudeState quat_copy &attitudeState.q1 q CrossProduct const float * &accelSensorData.x const float * grot accel_err accel_mag = accelSensorData.x * accelSensorData.x + accelSensorData.y * accelSensorData.y + accelSensorData.z * accelSensorData.z MagnetoSensorData mag MagnetoSensorGet &mag gyroSensorData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyroSensorData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyroSensorData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT qdot 0 = DEG2RAD -q 1 * gyroSensorData.x - q 2 * gyroSensorData.y - q 3 * gyroSensorData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyroSensorData.x - q 3 * gyroSensorData.y + q 2 * gyroSensorData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyroSensorData.x + q 0 * gyroSensorData.y - q 1 * gyroSensorData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyroSensorData.x + q 1 * gyroSensorData.y + q 0 * gyroSensorData.z * dT / 2 quat_copy q &attitudeState.q1 Quaternion2RPY &attitudeState.q1 &attitudeState.Roll AttitudeStateSet &attitudeState PositionStateData positionState PositionStateGet &positionState positionState.North = NED 0 positionState.East = NED 1 positionState.Down = NED 2 PositionStateSet &positionState VelocityStateData velocityState VelocityStateGet &velocityState velocityState.North = gpsVelocity.North velocityState.East = gpsVelocity.East velocityState.Down = gpsVelocity.Down VelocityStateSet &velocityState AirspeedStateData airspeed AirspeedStateGet &airspeed PositionStateData positionState PositionStateGet &positionState airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionState.Down AirspeedStateSet &airspeed GyroSensorData gyroSensorData AccelSensorData accelSensorData MagnetoSensorData magData BaroSensorData baroData if !AttitudeStateReadOnly GyroSensorGet &gyroSensorData AccelSensorGet &accelSensorData MagnetoSensorGet &magData BaroSensorGet &baroData if invalid gyroSensorData.x || invalid gyroSensorData.y || invalid gyroSensorData.z || invalid accelSensorData.x || invalid accelSensorData.y || invalid accelSensorData.z MagnetoSensorGet &magData AttitudeStateData attitudeState AttitudeStateGet &attitudeState attitudeState.Roll = atan2f -accelSensorData.y -accelSensorData.z float zn = cosf attitudeState.Roll * magData.z + sinf attitudeState.Roll * magData.y float yn = cosf attitudeState.Roll * magData.y - sinf attitudeState.Roll * magData.z float azn = cosf attitudeState.Roll * accelSensorData.z + sinf attitudeState.Roll * accelSensorData.y attitudeState.Pitch = atan2f accelSensorData.x -azn float xn = cosf attitudeState.Pitch * magData.x + sinf attitudeState.Pitch * zn attitudeState.Yaw = atan2f -yn xn attitudeState.Roll = RAD2DEG attitudeState.Roll attitudeState.Pitch = RAD2DEG attitudeState.Pitch attitudeState.Yaw = RAD2DEG attitudeState.Yaw RPY2Quaternion &attitudeState.Roll &attitudeState.q1 AttitudeStateSet &attitudeState float q 4 = attitudeState.q1 attitudeState.q2 attitudeState.q3 attitudeState.q4 float gyros 3 = DEG2RAD gyroSensorData.x DEG2RAD gyroSensorData.y DEG2RAD gyroSensorData.z INSStatePrediction gyros &accelSensorData.x dT AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude float gyros 3 = DEG2RAD gyroSensorData.x DEG2RAD gyroSensorData.y DEG2RAD gyroSensorData.z INSStatePrediction gyros &accelSensorData.x dT AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude AirspeedStateData airspeed AirspeedStateGet &airspeed AirspeedStateSet &airspeed PositionStateData positionState PositionStateGet &positionState positionState.North = Nav.Pos 0 positionState.East = Nav.Pos 1 positionState.Down = Nav.Pos 2 PositionStateSet &positionState VelocityStateData velocityState VelocityStateGet &velocityState velocityState.North = Nav.Vel 0 velocityState.East = Nav.Vel 1 velocityState.Down = Nav.Vel 2 VelocityStateSet &velocityState
LibrePilot 11210 attitude.c Non-Control 5284195c29 Refaktored sensor and state UAVObjects consistently into XXYSensor and XXYState * Specifically updates the the @ref AttitudeState AttitudeState and @ref AttitudeRaw AttitudeRaw settings objects * Output objects: @ref AttitudeRaw @ref AttitudeState #include gyrostate.h #include accelstate.h #include attitudestate.h static int32_t updateSensors AccelStateData * GyroStateData * static int32_t updateSensorsCC3D AccelStateData *accelStateData GyroStateData *gyrosData static void updateAttitude AccelStateData * GyroStateData * AttitudeStateInitialize AccelStateInitialize GyroStateInitialize AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude AccelStateData accelState GyroStateData gyros retval = updateSensorsCC3D &accelState &gyros retval = updateSensors &accelState &gyros if !AttitudeStateReadOnly updateAttitude &accelState &gyros static int32_t updateSensors AccelStateData *accelState GyroStateData *gyros if GyroStateReadOnly || AccelStateReadOnly accelState->x = vec_out 0 accelState->y = vec_out 1 accelState->z = vec_out 2 gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 accelState->x = accel 0 accelState->y = accel 1 accelState->z = accel 2 trim_accels 0 += accelState->x trim_accels 1 += accelState->y trim_accels 2 += accelState->z accelState->x = accelState->x - accelbias 0 * ACCEL_SCALE accelState->y = accelState->y - accelbias 1 * ACCEL_SCALE accelState->z = accelState->z - accelbias 2 * ACCEL_SCALE GyroStateSet gyros AccelStateSet accelState static int32_t updateSensorsCC3D AccelStateData *accelStateData GyroStateData *gyrosData if GyroStateReadOnly || AccelStateReadOnly accelStateData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelStateData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelStateData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 GyroStateSet gyrosData AccelStateSet accelStateData static void updateAttitude AccelStateData *accelStateData GyroStateData *gyrosData float *accels = &accelStateData->x AttitudeStateData attitudeState AttitudeStateGet &attitudeState quat_copy q &attitudeState.q1 Quaternion2RPY &attitudeState.q1 &attitudeState.Roll AttitudeStateSet &attitudeState * Specifically updates the the @ref AttitudeState AttitudeState and @ref AttitudeRaw AttitudeRaw settings objects * Output objects: @ref AttitudeRaw @ref AttitudeState #include accelsensor.h #include accelstate.h #include airspeedstate.h #include attitudestate.h #include barosensor.h #include gyrostate.h #include gyrosensor.h #include magnetosensor.h #include magnetostate.h #include positionstate.h #include velocitystate.h AttitudeStateInitialize AirspeedStateInitialize PositionStateInitialize VelocityStateInitialize AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude GyroSensorConnectQueue gyroQueue AccelSensorConnectQueue accelQueue MagnetoSensorConnectQueue magQueue BaroSensorConnectQueue baroQueue GyroSensorData gyroSensorData AccelSensorData accelSensorData if !AttitudeStateReadOnly AccelSensorGet &accelSensorData MagnetoSensorData magData MagnetoSensorGet &magData MagnetoSensorData magData AttitudeStateData attitudeState AttitudeStateGet &attitudeState attitudeState.Roll = atan2f -accelSensorData.y -accelSensorData.z float zn = cosf attitudeState.Roll * magData.z + sinf attitudeState.Roll * magData.y float yn = cosf attitudeState.Roll * magData.y - sinf attitudeState.Roll * magData.z float azn = cosf attitudeState.Roll * accelSensorData.z + sinf attitudeState.Roll * accelSensorData.y attitudeState.Pitch = atan2f accelSensorData.x -azn float xn = cosf attitudeState.Pitch * magData.x + sinf attitudeState.Pitch * zn attitudeState.Yaw = atan2f -yn xn attitudeState.Roll = RAD2DEG attitudeState.Roll attitudeState.Pitch = RAD2DEG attitudeState.Pitch attitudeState.Yaw = RAD2DEG attitudeState.Yaw RPY2Quaternion &attitudeState.Roll &attitudeState.q1 AttitudeStateSet &attitudeState GyroSensorGet &gyroSensorData AttitudeStateData attitudeState AttitudeStateGet &attitudeState quat_copy &attitudeState.q1 q CrossProduct const float * &accelSensorData.x const float * grot accel_err accel_mag = accelSensorData.x * accelSensorData.x + accelSensorData.y * accelSensorData.y + accelSensorData.z * accelSensorData.z MagnetoSensorData mag MagnetoSensorGet &mag gyroSensorData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyroSensorData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyroSensorData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT qdot 0 = DEG2RAD -q 1 * gyroSensorData.x - q 2 * gyroSensorData.y - q 3 * gyroSensorData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyroSensorData.x - q 3 * gyroSensorData.y + q 2 * gyroSensorData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyroSensorData.x + q 0 * gyroSensorData.y - q 1 * gyroSensorData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyroSensorData.x + q 1 * gyroSensorData.y + q 0 * gyroSensorData.z * dT / 2 quat_copy q &attitudeState.q1 Quaternion2RPY &attitudeState.q1 &attitudeState.Roll AttitudeStateSet &attitudeState PositionStateData positionState PositionStateGet &positionState positionState.North = NED 0 positionState.East = NED 1 positionState.Down = NED 2 PositionStateSet &positionState VelocityStateData velocityState VelocityStateGet &velocityState velocityState.North = gpsVelocity.North velocityState.East = gpsVelocity.East velocityState.Down = gpsVelocity.Down VelocityStateSet &velocityState AirspeedStateData airspeed AirspeedStateGet &airspeed PositionStateData positionState PositionStateGet &positionState airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionState.Down AirspeedStateSet &airspeed GyroSensorData gyroSensorData AccelSensorData accelSensorData MagnetoSensorData magData BaroSensorData baroData if !AttitudeStateReadOnly GyroSensorGet &gyroSensorData AccelSensorGet &accelSensorData MagnetoSensorGet &magData BaroSensorGet &baroData if invalid gyroSensorData.x || invalid gyroSensorData.y || invalid gyroSensorData.z || invalid accelSensorData.x || invalid accelSensorData.y || invalid accelSensorData.z MagnetoSensorGet &magData AttitudeStateData attitudeState AttitudeStateGet &attitudeState attitudeState.Roll = atan2f -accelSensorData.y -accelSensorData.z float zn = cosf attitudeState.Roll * magData.z + sinf attitudeState.Roll * magData.y float yn = cosf attitudeState.Roll * magData.y - sinf attitudeState.Roll * magData.z float azn = cosf attitudeState.Roll * accelSensorData.z + sinf attitudeState.Roll * accelSensorData.y attitudeState.Pitch = atan2f accelSensorData.x -azn float xn = cosf attitudeState.Pitch * magData.x + sinf attitudeState.Pitch * zn attitudeState.Yaw = atan2f -yn xn attitudeState.Roll = RAD2DEG attitudeState.Roll attitudeState.Pitch = RAD2DEG attitudeState.Pitch attitudeState.Yaw = RAD2DEG attitudeState.Yaw RPY2Quaternion &attitudeState.Roll &attitudeState.q1 AttitudeStateSet &attitudeState float q 4 = attitudeState.q1 attitudeState.q2 attitudeState.q3 attitudeState.q4 float gyros 3 = DEG2RAD gyroSensorData.x DEG2RAD gyroSensorData.y DEG2RAD gyroSensorData.z INSStatePrediction gyros &accelSensorData.x dT AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude float gyros 3 = DEG2RAD gyroSensorData.x DEG2RAD gyroSensorData.y DEG2RAD gyroSensorData.z INSStatePrediction gyros &accelSensorData.x dT AttitudeStateData attitude AttitudeStateGet &attitude AttitudeStateSet &attitude AirspeedStateData airspeed AirspeedStateGet &airspeed AirspeedStateSet &airspeed PositionStateData positionState PositionStateGet &positionState positionState.North = Nav.Pos 0 positionState.East = Nav.Pos 1 positionState.Down = Nav.Pos 2 PositionStateSet &positionState VelocityStateData velocityState VelocityStateGet &velocityState velocityState.North = Nav.Vel 0 velocityState.East = Nav.Vel 1 velocityState.Down = Nav.Vel 2 VelocityStateSet &velocityState
LibrePilot 11533 attitude.c Control 9c3a8369cd OP-946: State And Sensor Refaktoring: Moved Filtering for Magnetometers Gyros and Accels into Attitude static void magOffsetEstimation MagnetoSensorData *mag GyroSensorInitialize GyroStateInitialize AccelSensorInitialize AccelStateInitialize MagnetoSensorInitialize MagnetoStateInitialize AirspeedStateInitialize BaroSensorInitialize GPSPositionInitialize GPSVelocityInitialize AttitudeStateInitialize GyroStateData gyroStateData static float gyro_bias 3 = 0 0 0 AccelStateData accelState accelState.x = accelSensorData.x accelState.y = accelSensorData.y accelState.z = accelSensorData.z AccelStateSet &accelState gyroStateData.x = gyroSensorData.x gyroStateData.y = gyroSensorData.y gyroStateData.z = gyroSensorData.z if revoCalibration.MagBiasNullingRate > 0 magOffsetEstimation &mag MagnetoStateData mags mags.x = mag.x mags.y = mag.y mags.z = mag.z MagnetoStateSet &mags gyro_bias 0 -= accel_err 0 * attitudeSettings.AccelKi gyro_bias 0 -= accel_err 1 * attitudeSettings.AccelKi gyro_bias 0 -= mag_err 2 * magKi gyroStateData.x -= gyro_bias 0 gyroStateData.y -= gyro_bias 1 gyroStateData.z -= gyro_bias 2 GyroStateSet &gyroStateData gyroStateData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyroStateData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyroStateData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT qdot 0 = DEG2RAD -q 1 * gyroStateData.x - q 2 * gyroStateData.y - q 3 * gyroStateData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyroStateData.x - q 3 * gyroStateData.y + q 2 * gyroStateData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyroStateData.x + q 0 * gyroStateData.y - q 1 * gyroStateData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyroStateData.x + q 1 * gyroStateData.y + q 0 * gyroStateData.z * dT / 2 MagnetoStateData magData if mag_updated MagnetoSensorData mags MagnetoSensorGet &mags if revoCalibration.MagBiasNullingRate > 0 magOffsetEstimation &mags magData.x = mags.x magData.y = mags.y magData.z = mags.z MagnetoStateSet &magData MagnetoStateGet &magData AccelStateData accelState accelState.x = accelSensorData.x accelState.y = accelSensorData.y accelState.z = accelSensorData.z AccelStateSet &accelState GyroStateData gyroState gyroState.x = RAD2DEG gyros 0 - Nav.gyro_bias 0 gyroState.y = RAD2DEG gyros 1 - Nav.gyro_bias 1 gyroState.z = RAD2DEG gyros 2 - Nav.gyro_bias 2 GyroStateSet &gyroState static void magOffsetEstimation MagnetoSensorData *mag #if 0 static const float MIN_NORM_DIFFERENCE = 50 static float B2 3 = 0 0 0 MagBiasData magBias MagBiasGet &magBias mag->x -= magBias.x mag->y -= magBias.y mag->z -= magBias.z if B2 0 == 0 && B2 1 == 0 && B2 2 == 0 B2 0 = mag->x B2 1 = mag->y B2 2 = mag->z return float B1 3 = mag->x mag->y mag->z float norm_diff = sqrtf powf B2 0 - B1 0 2 + powf B2 1 - B1 1 2 + powf B2 2 - B1 2 2 if norm_diff > MIN_NORM_DIFFERENCE float norm_b1 = sqrtf B1 0 * B1 0 + B1 1 * B1 1 + B1 2 * B1 2 float norm_b2 = sqrtf B2 0 * B2 0 + B2 1 * B2 1 + B2 2 * B2 2 float scale = cal.MagBiasNullingRate * norm_b2 - norm_b1 / norm_diff float b_error 3 = B2 0 - B1 0 * scale B2 1 - B1 1 * scale B2 2 - B1 2 * scale magBias.x += b_error 0 magBias.y += b_error 1 magBias.z += b_error 2 MagBiasSet &magBias B2 0 = B1 0 B2 1 = B1 1 B2 2 = B1 2 #else static float magBias 3 = 0 mag->x -= magBias 0 mag->y -= magBias 1 mag->z -= magBias 2 AttitudeStateData attitude AttitudeStateGet &attitude const float Rxy = sqrtf homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 const float Rz = homeLocation.Be 2 const float rate = revoCalibration.MagBiasNullingRate float Rot 3 3 float B_e 3 float xy 2 float delta 3 Quaternion2R &attitude.q1 Rot B_e 0 = Rot 0 0 * mag->x + Rot 1 0 * mag->y + Rot 2 0 * mag->z B_e 1 = Rot 0 1 * mag->x + Rot 1 1 * mag->y + Rot 2 1 * mag->z B_e 2 = Rot 0 2 * mag->x + Rot 1 2 * mag->y + Rot 2 2 * mag->z float cy = cosf DEG2RAD attitude.Yaw float sy = sinf DEG2RAD attitude.Yaw xy 0 = cy * B_e 0 + sy * B_e 1 xy 1 = -sy * B_e 0 + cy * B_e 1 float xy_norm = sqrtf xy 0 * xy 0 + xy 1 * xy 1 delta 0 = -rate * xy 0 / xy_norm * Rxy - xy 0 delta 1 = -rate * xy 1 / xy_norm * Rxy - xy 1 delta 2 = -rate * Rz - B_e 2 if !isnan delta 0 && !isinf delta 0 && !isnan delta 1 && !isinf delta 1 && !isnan delta 2 && !isinf delta 2 magBias 0 += delta 0 magBias 1 += delta 1 magBias 2 += delta 2 #endif
LibrePilot 11562 attitude.c Non-Control daf329d8f5 uncrustification (again) #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 11563 attitude.c Non-Control daf329d8f5 uncrustification (again) #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 11821 attitude.c Non-Control 900f643bbd Reformat source code with make uncrustify_all run twice. NO CODE CHANGES #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 11822 attitude.c Non-Control 900f643bbd Reformat source code with make uncrustify_all run twice. NO CODE CHANGES #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 static float gyro_correct_int 3 = 0 0 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData static void settingsUpdatedCb UAVObjEvent *objEv static float accelKi = 0 static float accelKp = 0 static float gyroGain = 0.42f static float q 4 = 1 0 0 0 static bool bias_correct_gyro = true static volatile bool trim_requested = false #define GRAV 9.81f #define ACCEL_SCALE GRAV * 0.004f xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE taskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE return 0 AttitudeActualInitialize AttitudeSettingsInitialize AccelsInitialize GyrosInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1 attitude.q2 = 0 attitude.q3 = 0 attitude.q4 = 0 AttitudeActualSet &attitude gyro_correct_int 0 = 0 gyro_correct_int 1 = 0 gyro_correct_int 2 = 0 q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 for uint8_t i = 0 i < 3 i++ for uint8_t j = 0 j < 3 j++ R i j = 0 trim_requested = false AttitudeSettingsConnectCallback &settingsUpdatedCb return 0 uint8_t init = 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE const struct pios_board_info *bdinfo = &pios_board_info_blob bool cc3d = bdinfo->board_rev == 0x02 if cc3d gyro_test = PIOS_MPU6000_Test else accel_test = PIOS_ADXL345_Test gyro_queue = xQueueCreate 1 sizeof float * 4 PIOS_Assert gyro_queue != NULL PIOS_ADC_SetQueue gyro_queue PIOS_ADC_Config PIOS_ADC_RATE / 1000.0f * UPDATE_RATE settingsUpdatedCb AttitudeSettingsHandle while 1 FlightStatusData flightStatus FlightStatusGet &flightStatus if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if zero_during_arming && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING accelKp = 1.0f accelKi = 0.0f yawBiasRate = 0.01f rollPitchBiasRate = 0.01f accel_filter_enabled = false init = 0 else if init == 0 AttitudeSettingsAccelKiGet &accelKi AttitudeSettingsAccelKpGet &accelKp AttitudeSettingsYawBiasRateGet &yawBiasRate rollPitchBiasRate = 0.0f if accel_alpha > 0.0f accel_filter_enabled = true init = 1 PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE AccelsData accels GyrosData gyros int32_t retval = 0 if cc3d retval = updateSensorsCC3D &accels &gyros else retval = updateSensors &accels &gyros if retval != 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if !AttitudeActualReadOnly updateAttitude &accels &gyros AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE static int32_t updateSensors AccelsData *accels GyrosData *gyros struct pios_adxl345_data accel_data float gyro 4 if xQueueReceive gyro_queue void *const gyro UPDATE_RATE * 2 == errQUEUE_EMPTY AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return -1 if GyrosReadOnly || AccelsReadOnly return 0 if PIOS_ADXL345_FifoElements == 0 return -1 gyros->x = - gyro 1 - GYRO_NEUTRAL * gyroGain gyros->y = gyro 2 - GYRO_NEUTRAL * gyroGain gyros->z = - gyro 3 - GYRO_NEUTRAL * gyroGain int32_t x = 0 int32_t y = 0 int32_t z = 0 uint8_t i = 0 uint8_t samples_remaining do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 gyros->temperature = samples_remaining float accel 3 = float x / i float y / i float z / i if rotate float vec_out 3 rot_mult R accel vec_out accels->x = vec_out 0 accels->y = vec_out 1 accels->z = vec_out 2 rot_mult R &gyros->x vec_out gyros->x = vec_out 0 gyros->y = vec_out 1 gyros->z = vec_out 2 else accels->x = accel 0 accels->y = accel 1 accels->z = accel 2 if trim_requested if trim_samples >= MAX_TRIM_FLIGHT_SAMPLES trim_requested = false else uint8_t armed float throttle FlightStatusArmedGet &armed ManualControlCommandThrottleGet &throttle if armed == FLIGHTSTATUS_ARMED_ARMED && throttle > 0.0f trim_samples++ trim_accels 0 += accels->x trim_accels 1 += accels->y trim_accels 2 += accels->z accels->x = accels->x - accelbias 0 * ACCEL_SCALE accels->y = accels->y - accelbias 1 * ACCEL_SCALE accels->z = accels->z - accelbias 2 * ACCEL_SCALE if bias_correct_gyro gyros->x += gyro_correct_int 0 gyros->y += gyro_correct_int 1 gyros->z += gyro_correct_int 2 gyro_correct_int 0 += -gyros->x * rollPitchBiasRate gyro_correct_int 1 += -gyros->y * rollPitchBiasRate gyro_correct_int 2 += -gyros->z * yawBiasRate GyrosSet gyros AccelsSet accels return 0 static int32_t updateSensorsCC3D AccelsData *accelsData GyrosData *gyrosData float accels 3 gyros 3 xQueueHandle queue = PIOS_MPU6000_GetQueue if xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD == errQUEUE_EMPTY return -1 if GyrosReadOnly || AccelsReadOnly return 0 gyros 0 = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale gyros 1 = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale gyros 2 = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale accels 0 = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale accels 1 = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale accels 2 = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale gyrosData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f accelsData->temperature = 35.0f + float mpu6000_data.temperature + 512.0f / 340.0f #endif if rotate float vec_out 3 rot_mult R accels vec_out accels 0 = vec_out 0 accels 1 = vec_out 1 accels 2 = vec_out 2 rot_mult R gyros vec_out gyros 0 = vec_out 0 gyros 1 = vec_out 1 gyros 2 = vec_out 2 accelsData->x = accels 0 - accelbias 0 * ACCEL_SCALE accelsData->y = accels 1 - accelbias 1 * ACCEL_SCALE accelsData->z = accels 2 - accelbias 2 * ACCEL_SCALE gyrosData->x = gyros 0 gyrosData->y = gyros 1 gyrosData->z = gyros 2 if bias_correct_gyro gyrosData->x += gyro_correct_int 0 gyrosData->y += gyro_correct_int 1 gyrosData->z += gyro_correct_int 2 gyro_correct_int 0 += -gyrosData->x * rollPitchBiasRate gyro_correct_int 1 += -gyrosData->y * rollPitchBiasRate gyro_correct_int 2 += -gyrosData->z * yawBiasRate GyrosSet gyrosData AccelsSet accelsData return 0 if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 static void updateAttitude AccelsData *accelsData GyrosData *gyrosData float dT portTickType thisSysTime = xTaskGetTickCount static portTickType lastSysTime = 0 dT = thisSysTime == lastSysTime ? 0.001f : thisSysTime - lastSysTime * portTICK_RATE_MS * 0.001f lastSysTime = thisSysTime float *gyros = &gyrosData->x float *accels = &accelsData->x float grot 3 float accel_err 3 apply_accel_filter accels accels_filtered grot 0 = - 2 * q 1 * q 3 - q 0 * q 2 grot 1 = - 2 * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err float accel_mag = sqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if accel_mag < 1.0e-3f return float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f if grot_mag < 1.0e-3f return accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyro_correct_int 0 += accel_err 0 * accelKi gyro_correct_int 1 += accel_err 1 * accelKi gyros 0 += accel_err 0 * accelKp / dT gyros 1 += accel_err 1 * accelKp / dT gyros 2 += accel_err 2 * accelKp / dT float qdot 4 qdot 0 = -q 1 * gyros 0 - q 2 * gyros 1 - q 3 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros 0 - q 3 * gyros 1 + q 2 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros 0 + q 0 * gyros 1 - q 1 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros 0 + q 1 * gyros 1 + q 0 * gyros 2 * dT * M_PI_F / 180.0f / 2.0f q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0 q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 float qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf qmag < 1e-3f || isnan qmag q 0 = 1 q 1 = 0 q 2 = 0 q 3 = 0 else q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual static void settingsUpdatedCb __attribute__ unused UAVObjEvent *objEv AttitudeSettingsData attitudeSettings AttitudeSettingsGet &attitudeSettings accelKp = attitudeSettings.AccelKp accelKi = attitudeSettings.AccelKi yawBiasRate = attitudeSettings.YawBiasRate gyroGain = attitudeSettings.GyroGain const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE accelbias 0 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X accelbias 1 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y accelbias 2 = attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z gyro_correct_int 0 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias ATTITUDESETTINGS_GYROBIAS_Z / 100.0f if attitudeSettings.BoardRotation 0 == 0 && attitudeSettings.BoardRotation 1 == 0 && attitudeSettings.BoardRotation 2 == 0 rotate = 0 float rotationQuat 4 = 1 0 0 0 Quaternion2R rotationQuat R else float rotationQuat 4 const float rpy 3 = attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_ROLL attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_PITCH attitudeSettings.BoardRotation ATTITUDESETTINGS_BOARDROTATION_YAW RPY2Quaternion rpy rotationQuat Quaternion2R rotationQuat R rotate = 1 if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START trim_accels 0 = 0 trim_accels 1 = 0 trim_accels 2 = 0 trim_samples = 0 trim_requested = true else if attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD trim_requested = false attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_X = trim_accels 0 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias ATTITUDESETTINGS_ACCELBIAS_Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL AttitudeSettingsSet &attitudeSettings else trim_requested = false #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f #define IAS2TAS alt 1.0f + 0.02f * alt / 304.8f const uint32_t SENSOR_QUEUE_SIZE = 10 static uint32_t volatile running_algorithm = 0xffffffff static void settingsUpdatedCb UAVObjEvent *objEv static int32_t getNED GPSPositionData *gpsPosition float *NED static inline bool invalid float data if isnan data || isinf data return true return false static inline bool invalid_var float data if invalid data return true if data < 1e-15f return true return false AttitudeActualInitialize AirspeedActualInitialize AirspeedSensorInitialize AttitudeSettingsInitialize PositionActualInitialize VelocityActualInitialize RevoSettingsInitialize RevoCalibrationInitialize EKFConfigurationInitialize EKFStateVarianceInitialize FlightStatusInitialize HomeLocationInitialize AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = 1.0f attitude.q2 = 0.0f attitude.q3 = 0.0f attitude.q4 = 0.0f AttitudeActualSet &attitude GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f GyrosBiasSet &gyrosBias AttitudeSettingsConnectCallback &settingsUpdatedCb RevoSettingsConnectCallback &settingsUpdatedCb RevoCalibrationConnectCallback &settingsUpdatedCb HomeLocationConnectCallback &settingsUpdatedCb EKFConfigurationConnectCallback &settingsUpdatedCb FlightStatusConnectCallback &settingsUpdatedCb return 0 gyroQueue = xQueueCreate 1 sizeof UAVObjEvent accelQueue = xQueueCreate 1 sizeof UAVObjEvent magQueue = xQueueCreate 1 sizeof UAVObjEvent airspeedQueue = xQueueCreate 1 sizeof UAVObjEvent baroQueue = xQueueCreate 1 sizeof UAVObjEvent gpsQueue = xQueueCreate 1 sizeof UAVObjEvent gpsVelQueue = xQueueCreate 1 sizeof UAVObjEvent xTaskCreate AttitudeTask signed char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle PIOS_TASK_MONITOR_RegisterTask TASKINFO_RUNNING_ATTITUDE attitudeTaskHandle PIOS_WDG_RegisterFlag PIOS_WDG_ATTITUDE GyrosConnectQueue gyroQueue AccelsConnectQueue accelQueue MagnetometerConnectQueue magQueue AirspeedSensorConnectQueue airspeedQueue BaroAltitudeConnectQueue baroQueue GPSPositionConnectQueue gpsQueue GPSVelocityConnectQueue gpsVelQueue return 0 AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE settingsUpdatedCb NULL vTaskDelay 100 while 1 int32_t ret_val = -1 bool first_run = false if initialization_required initialization_required = false first_run = true switch running_algorithm case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary first_run break case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS first_run true break case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS first_run false break default: AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL break if ret_val != 0 initialization_required = true PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE UAVObjEvent ev GyrosData gyrosData AccelsData accelsData static int32_t timeval float dT static uint8_t init = 0 if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 AccelsGet &accelsData if first_run if xQueueReceive magQueue &ev 0 / portTICK_RATE_MS != pdTRUE return -1 MagnetometerData magData MagnetometerGet &magData MagnetometerData magData magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual init = 0 attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual timeval = PIOS_DELAY_GetRaw return 0 if init == 0 && xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f else if attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE && flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING attitudeSettings.AccelKp = 1.0f attitudeSettings.AccelKi = 0.9f attitudeSettings.YawBiasRate = 0.23f magKp = 1.0f init = 0 else if init == 0 AttitudeSettingsGet &attitudeSettings magKp = 0.01f init = 1 GyrosGet &gyrosData dT = PIOS_DELAY_DiffuS timeval / 1000000.0f timeval = PIOS_DELAY_GetRaw float q 4 AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual float grot 3 float accel_err 3 quat_copy &attitudeActual.q1 q grot 0 = - 2.0f * q 1 * q 3 - q 0 * q 2 grot 1 = - 2.0f * q 2 * q 3 + q 0 * q 1 grot 2 = - q 0 * q 0 - q 1 * q 1 - q 2 * q 2 + q 3 * q 3 CrossProduct const float * &accelsData.x const float * grot accel_err accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z accel_mag = sqrtf accel_mag accel_err 0 /= accel_mag accel_err 1 /= accel_mag accel_err 2 /= accel_mag if xQueueReceive magQueue &ev 0 != pdTRUE float brot 3 float Rbe 3 3 MagnetometerData mag Quaternion2R q Rbe MagnetometerGet &mag if !isnan mag.x && !isinf mag.x && !isnan mag.y && !isinf mag.y && !isnan mag.z && !isinf mag.z rot_mult Rbe homeLocation.Be brot float mag_len = sqrtf mag.x * mag.x + mag.y * mag.y + mag.z * mag.z mag.x /= mag_len mag.y /= mag_len mag.z /= mag_len float bmag = sqrtf brot 0 * brot 0 + brot 1 * brot 1 + brot 2 * brot 2 brot 0 /= bmag brot 1 /= bmag brot 2 /= bmag if bmag < 1.0f || mag_len < 1.0f mag_err 0 = mag_err 1 = mag_err 2 = 0.0f else CrossProduct const float * &mag.x const float * brot mag_err else mag_err 0 = mag_err 1 = mag_err 2 = 0.0f GyrosBiasData gyrosBias GyrosBiasGet &gyrosBias gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi gyrosBias.z -= mag_err 2 * magKi GyrosBiasSet &gyrosBias if revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosData.x -= gyrosBias.x gyrosData.y -= gyrosBias.y gyrosData.z -= gyrosBias.z gyrosData.x += accel_err 0 * attitudeSettings.AccelKp / dT gyrosData.y += accel_err 1 * attitudeSettings.AccelKp / dT gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * magKp / dT float qdot 4 qdot 0 = DEG2RAD -q 1 * gyrosData.x - q 2 * gyrosData.y - q 3 * gyrosData.z * dT / 2 qdot 1 = DEG2RAD q 0 * gyrosData.x - q 3 * gyrosData.y + q 2 * gyrosData.z * dT / 2 qdot 2 = DEG2RAD q 3 * gyrosData.x + q 0 * gyrosData.y - q 1 * gyrosData.z * dT / 2 qdot 3 = DEG2RAD -q 2 * gyrosData.x + q 1 * gyrosData.y + q 0 * gyrosData.z * dT / 2 q 0 = q 0 + qdot 0 q 1 = q 1 + qdot 1 q 2 = q 2 + qdot 2 q 3 = q 3 + qdot 3 if q 0 < 0.0f q 0 = -q 0 q 1 = -q 1 q 2 = -q 2 q 3 = -q 3 qmag = sqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 q 0 = q 0 / qmag q 1 = q 1 / qmag q 2 = q 2 / qmag q 3 = q 3 / qmag if fabsf qmag < 1.0e-3f || isnan qmag q 0 = 1.0f q 1 = 0.0f q 2 = 0.0f q 3 = 0.0f quat_copy q &attitudeActual.q1 Quaternion2RPY &attitudeActual.q1 &attitudeActual.Roll AttitudeActualSet &attitudeActual xQueueReceive baroQueue &ev 0 if xQueueReceive gpsQueue &ev 0 == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE float NED 3 GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition NED PositionActualData positionActual PositionActualGet &positionActual positionActual.North = NED 0 positionActual.East = NED 1 positionActual.Down = NED 2 PositionActualSet &positionActual if xQueueReceive gpsVelQueue &ev 0 == pdTRUE GPSVelocityData gpsVelocity GPSVelocityGet &gpsVelocity VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = gpsVelocity.North velocityActual.East = gpsVelocity.East velocityActual.Down = gpsVelocity.Down VelocityActualSet &velocityActual if xQueueReceive airspeedQueue &ev 0 == pdTRUE AirspeedSensorData airspeedSensor AirspeedSensorGet &airspeedSensor AirspeedActualData airspeed AirspeedActualGet &airspeed PositionActualData positionActual PositionActualGet &positionActual if airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionActual.Down AirspeedActualSet &airspeed if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE return 0 UAVObjEvent ev GyrosData gyrosData AccelsData accelsData MagnetometerData magData AirspeedSensorData airspeedData BaroAltitudeData baroData GPSPositionData gpsData GPSVelocityData gpsVelData GyrosBiasData gyrosBias static bool mag_updated = false static bool baro_updated static bool airspeed_updated static bool gps_updated static bool gps_vel_updated static bool value_error = false static float baroOffset = 0.0f static uint32_t ins_last_time = 0 static bool inited float NED 3 = 0.0f 0.0f 0.0f float vel 3 = 0.0f 0.0f 0.0f float zeros 3 = 0.0f 0.0f 0.0f uint16_t sensors = 0 float dT if xQueueReceive gyroQueue &ev FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS != pdTRUE || xQueueReceive accelQueue &ev 1 / portTICK_RATE_MS != pdTRUE if !AttitudeActualReadOnly AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_WARNING return -1 if inited mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 if first_run inited = false init_stage = 0 mag_updated = 0 baro_updated = 0 airspeed_updated = 0 gps_updated = 0 gps_vel_updated = 0 ins_last_time = PIOS_DELAY_GetRaw return 0 mag_updated |= xQueueReceive magQueue &ev 0 / portTICK_RATE_MS == pdTRUE baro_updated |= xQueueReceive baroQueue &ev 0 / portTICK_RATE_MS == pdTRUE airspeed_updated |= xQueueReceive airspeedQueue &ev 0 / portTICK_RATE_MS == pdTRUE if !GPSPositionReadOnly gps_updated |= xQueueReceive gpsQueue &ev 0 / portTICK_RATE_MS == pdTRUE && outdoor_mode else gps_updated |= pdTRUE && outdoor_mode GyrosGet &gyrosData AccelsGet &accelsData MagnetometerGet &magData BaroAltitudeGet &baroData AirspeedSensorGet &airspeedData GPSPositionGet &gpsData GPSVelocityGet &gpsVelData GyrosBiasGet &gyrosBias value_error = false if invalid gyrosData.x || invalid gyrosData.y || invalid gyrosData.z || invalid accelsData.x || invalid accelsData.y || invalid accelsData.z AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR return 0 if invalid gyrosBias.x || invalid gyrosBias.y || invalid gyrosBias.z gyrosBias.x = 0.0f gyrosBias.y = 0.0f gyrosBias.z = 0.0f if invalid magData.x || invalid magData.y || invalid magData.z mag_updated = false value_error = true if homeLocation.Be 0 * homeLocation.Be 0 + homeLocation.Be 1 * homeLocation.Be 1 + homeLocation.Be 2 * homeLocation.Be 2 < 1e-5f mag_updated = false value_error = true if invalid baroData.Altitude baro_updated = false value_error = true if invalid airspeedData.CalibratedAirspeed airspeed_updated = false value_error = true if invalid gpsData.Altitude gps_updated = false value_error = true if invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST || invalid_var ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN gps_updated = false value_error = true if invalid gpsVelData.North || invalid gpsVelData.East || invalid gpsVelData.Down gps_vel_updated = false value_error = true if airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE airspeed_updated = false if gpsData.Satellites < 7 || gpsData.PDOP > 4.0f || gpsData.Latitude == 0 && gpsData.Longitude == 0 || homeLocation.Set != HOMELOCATION_SET_TRUE gps_updated = false gps_vel_updated = false if !inited AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if value_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if variance_error AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL else if outdoor_mode && gpsData.Satellites < 7 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else AlarmsClear SYSTEMALARMS_ALARM_ATTITUDE dT = PIOS_DELAY_DiffuS ins_last_time / 1.0e6f ins_last_time = PIOS_DELAY_GetRaw if dT > 0.01f dT = 0.01f else if dT <= 0.001f dT = 0.001f if !inited && mag_updated && baro_updated && gps_updated || !outdoor_mode && !variance_error if init_stage == 0 INSGPSInit INSSetMagVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_MAGX ekfConfiguration.R EKFCONFIGURATION_R_MAGY ekfConfiguration.R EKFCONFIGURATION_R_MAGZ INSSetAccelVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELX ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELY ekfConfiguration.Q EKFCONFIGURATION_Q_ACCELZ INSSetGyroVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYROX ekfConfiguration.Q EKFCONFIGURATION_Q_GYROY ekfConfiguration.Q EKFCONFIGURATION_Q_GYROZ INSSetGyroBiasVar float 3 ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTX ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTY ekfConfiguration.Q EKFCONFIGURATION_Q_GYRODRIFTZ INSSetBaroVar ekfConfiguration.R EKFCONFIGURATION_R_BAROZ float gyro_bias 3 = 0.0f 0.0f 0.0f INSSetGyroBias gyro_bias float pos 3 = 0.0f 0.0f 0.0f if outdoor_mode GPSPositionData gpsPosition GPSPositionGet &gpsPosition getNED &gpsPosition pos baroOffset = -pos 2 - baroData.Altitude else baroOffset = -baroData.Altitude pos 2 = - baroData.Altitude + baroOffset xQueueReceive magQueue &ev 100 / portTICK_RATE_MS MagnetometerGet &magData AttitudeActualData attitudeActual AttitudeActualGet &attitudeActual attitudeActual.Roll = atan2f -accelsData.y -accelsData.z float zn = cosf attitudeActual.Roll * magData.z + sinf attitudeActual.Roll * magData.y float yn = cosf attitudeActual.Roll * magData.y - sinf attitudeActual.Roll * magData.z float azn = cosf attitudeActual.Roll * accelsData.z + sinf attitudeActual.Roll * accelsData.y attitudeActual.Pitch = atan2f accelsData.x -azn float xn = cosf attitudeActual.Pitch * magData.x + sinf attitudeActual.Pitch * zn attitudeActual.Yaw = atan2f -yn xn attitudeActual.Roll = RAD2DEG attitudeActual.Roll attitudeActual.Pitch = RAD2DEG attitudeActual.Pitch attitudeActual.Yaw = RAD2DEG attitudeActual.Yaw RPY2Quaternion &attitudeActual.Roll &attitudeActual.q1 AttitudeActualSet &attitudeActual float q 4 = attitudeActual.q1 attitudeActual.q2 attitudeActual.q3 attitudeActual.q4 INSSetState pos zeros q zeros zeros INSResetP ekfConfiguration.P else float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude init_stage++ if init_stage > 10 inited = true return 0 if !inited return 0 float gyros 3 = DEG2RAD gyrosData.x DEG2RAD gyrosData.y DEG2RAD gyrosData.z if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyros 0 += DEG2RAD gyrosBias.x gyros 1 += DEG2RAD gyrosBias.y gyros 2 += DEG2RAD gyrosBias.z INSStatePrediction gyros &accelsData.x dT AttitudeActualData attitude AttitudeActualGet &attitude attitude.q1 = Nav.q 0 attitude.q2 = Nav.q 1 attitude.q3 = Nav.q 2 attitude.q4 = Nav.q 3 Quaternion2RPY &attitude.q1 &attitude.Roll AttitudeActualSet &attitude INSCovariancePrediction dT if mag_updated sensors |= MAG_SENSORS if baro_updated sensors |= BARO_SENSOR INSSetMagNorth homeLocation.Be if gps_updated && outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSPOSDOWN float 3 ekfConfiguration.R EKFCONFIGURATION_R_GPSVELNORTH ekfConfiguration.R EKFCONFIGURATION_R_GPSVELEAST ekfConfiguration.R EKFCONFIGURATION_R_GPSVELDOWN sensors |= POS_SENSORS if 0 sensors |= HORIZ_SENSORS vel 0 = gpsData.Groundspeed * cosf DEG2RAD gpsData.Heading vel 1 = gpsData.Groundspeed * sinf DEG2RAD gpsData.Heading vel 2 = 0.0f getNED &gpsData NED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -NED 2 - baroData.Altitude else if !outdoor_mode INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR vel 0 = vel 1 = vel 2 = 0.0f NED 0 = NED 1 = 0.0f NED 2 = - baroData.Altitude + baroOffset sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS sensors |= POS_SENSORS | VERT_SENSORS if gps_vel_updated && outdoor_mode sensors |= HORIZ_SENSORS | VERT_SENSORS vel 0 = gpsVelData.North vel 1 = gpsVelData.East vel 2 = gpsVelData.Down if airspeed_updated AirspeedActualData airspeed AirspeedActualGet &airspeed airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - Nav.Pos 2 AirspeedActualSet &airspeed if !gps_vel_updated && !gps_updated sensors |= HORIZ_SENSORS | VERT_SENSORS INSSetPosVelVar float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR float 3 ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED ekfConfiguration.FakeR EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED float R 3 3 Quaternion2R Nav.q R float vtas 3 = airspeed.TrueAirspeed 0.0f 0.0f rot_mult R vtas vel if sensors INSCorrection &magData.x NED vel baroData.Altitude + baroOffset sensors PositionActualData positionActual PositionActualGet &positionActual positionAc ual.North = Nav.Pos 0 positionActual.East = Nav.Pos 1 positionActual.Down = Nav.Pos 2 PositionActualSet &positionActual VelocityActualData velocityActual VelocityActualGet &velocityActual velocityActual.North = Nav.Vel 0 velocityActual.East = Nav.Vel 1 velocityActual.Down = Nav.Vel 2 VelocityActualSet &velocityActual gyrosBias.x = RAD2DEG Nav.gyro_bias 0 gyrosBias.y = RAD2DEG Nav.gyro_bias 1 gyrosBias.z = RAD2DEG Nav.gyro_bias 2 GyrosBiasSet &gyrosBias EKFStateVarianceData vardata EKFStateVarianceGet &vardata INSGetP vardata.P EKFStateVarianceSet &vardata return 0 static int32_t getNED GPSPositionData *gpsPosition float *NED float dL 3 = DEG2RAD gpsPosition->Latitude - homeLocation.Latitude / 10.0e6f DEG2RAD gpsPosition->Longitude - homeLocation.Longitude / 10.0e6f gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude NED 0 = T 0 * dL 0 NED 1 = T 1 * dL 1 NED 2 = T 2 * dL 2 return 0 static void settingsUpdatedCb UAVObjEvent *ev if ev == NULL || ev->obj == FlightStatusHandle FlightStatusGet &flightStatus if ev == NULL || ev->obj == RevoCalibrationHandle RevoCalibrationGet &revoCalibration if ev == NULL || ev->obj == EKFConfigurationHandle || ev->obj == RevoSettingsHandle || variance_error == true && ev->obj == FlightStatusHandle bool error = false EKFConfigurationGet &ekfConfiguration int t for t = 0 t < EKFCONFIGURATION_P_NUMELEM t++ if invalid_var ekfConfiguration.P t error = true for t = 0 t < EKFCONFIGURATION_Q_NUMELEM t++ if invalid_var ekfConfiguration.Q t error = true for t = 0 t < EKFCONFIGURATION_R_NUMELEM t++ if invalid_var ekfConfiguration.R t error = true RevoSettingsGet &revoSettings if flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required error = true if error variance_error = true else running_algorithm = revoSettings.FusionAlgorithm variance_error = false initialization_required = true if ev == NULL || ev->obj == HomeLocationHandle HomeLocationGet &homeLocation float lat alt lat = DEG2RAD homeLocation.Latitude / 10.0e6f alt = homeLocation.Altitude T 0 = alt + 6.378137E6f T 1 = cosf lat * alt + 6.378137E6f T 2 = -1.0f if ev == NULL || ev->obj == AttitudeSettingsHandle AttitudeSettingsGet &attitudeSettings
LibrePilot 12049 attitude.c Non-Control 649667d338 Addes ifdef PIOS_INCLUDE_WDG around several calls to the wathdog functions for the Revolution firmware (allows you to turn off the watchdog). #ifdef PIOS_INCLUDE_WDG #endif #ifdef PIOS_INCLUDE_WDG #endif
LibrePilot 12060 attitude.c Non-Control 1236bf3ed9 renamed Magneto to Mag #include magsensor.h #include magstate.h static void magOffsetEstimation MagSensorData *mag MagSensorInitialize MagStateInitialize MagSensorConnectQueue magQueue MagSensorData magData MagSensorGet &magData MagSensorData magData MagSensorData mag MagSensorGet &mag MagStateData mags MagStateSet &mags MagStateData magData MagSensorData mags MagSensorGet &mags MagStateSet &magData MagStateGet &magData * Magmeter Offset Cancellation: Theory and Implementation static void magOffsetEstimation MagSensorData *mag
LibrePilot 12080 attitude.c Control 3cb4f59c94 failsafe fallback to attitude for now until StateEstimation works reliably case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
LibrePilot 12081 attitude.c Control fb433a97d7 Bugfix: add gyro bias to correct axis gyro_bias 1 -= accel_err 1 * attitudeSettings.AccelKi gyro_bias 2 -= mag_err 2 * magKi
LibrePilot 12087 attitude.c Control 464d2be9f7 Port OP-754 to Revolution attitude estimation module static float rollPitchBiasRate = 0 attitudeSettings.AccelKi = 0.0f rollPitchBiasRate = 0.01f attitudeSettings.AccelKi = 0.0f rollPitchBiasRate = 0.01f rollPitchBiasRate = 0.0f gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi + gyrosData.y * rollPitchBiasRate
LibrePilot 12088 attitude.c Control 0b5a28f19e Port OP-754 to Revolution attitude estimation module: subtract GyroBIAS to work on raw valueAdded MagKp and MagKi to AttitudeSetting UAVO attitudeSettings.MagKp = 1.0f attitudeSettings.MagKp = 1.0f if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi + gyrosData.x + gyrosBias.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi + gyrosData.y + gyrosBias.y * rollPitchBiasRate else gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi + gyrosData.y * rollPitchBiasRate gyrosBias.z -= mag_err 2 * attitudeSettings.MagKi GyrosBiasSet &gyrosBias gyrosData.z += accel_err 2 * attitudeSettings.AccelKp / dT + mag_err 2 * attitudeSettings.MagKp / dT
LibrePilot 12089 attitude.c Control 294a295510 Port CF patch from Cyr that filters accel to reduce attitude drift due to vibrations static float accel_alpha = 0 static bool accel_filter_enabled = false static float accels_filtered 3 static float grot_filtered 3 static inline void apply_accel_filter const float *raw float *filtered if accel_filter_enabled filtered 0 = filtered 0 * accel_alpha + raw 0 * 1 - accel_alpha filtered 1 = filtered 1 * accel_alpha + raw 1 * 1 - accel_alpha filtered 2 = filtered 2 * accel_alpha + raw 2 * 1 - accel_alpha else filtered 0 = raw 0 filtered 1 = raw 1 filtered 2 = raw 2 accel_filter_enabled = false accel_filter_enabled = false if accel_alpha > 0.0f accel_filter_enabled = true apply_accel_filter const float * &accelsData.x accels_filtered apply_accel_filter grot grot_filtered CrossProduct const float * accels_filtered const float * grot_filtered accel_err accel_mag = accels_filtered 0 *accels_filtered 0 + accels_filtered 1 *accels_filtered 1 + accels_filtered 2 *accels_filtered 2 float grot_mag if accel_filter_enabled grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi + gyrosData.x + gyrosBias.x * rollPitchBiasRate const float fakeDt = 0.0025f if attitudeSettings.AccelTau < 0.0001f accel_alpha = 0 accel_filter_enabled = false else accel_alpha = expf -fakeDt / attitudeSettings.AccelTau accel_filter_enabled = true
LibrePilot 12104 attitude.c Non-Control 9b95af2006 refaktored GPS Sensor UAVObjects #include gpspositionsensor.h #include gpsvelocitysensor.h static int32_t getNED GPSPositionSensorData *gpsPosition float *NED GPSPositionSensorInitialize GPSVelocitySensorInitialize GPSPositionSensorConnectQueue gpsQueue GPSVelocitySensorConnectQueue gpsVelQueue GPSPositionSensorData gpsPosition GPSPositionSensorGet &gpsPosition GPSVelocitySensorData gpsVelocity GPSVelocitySensorGet &gpsVelocity GPSPositionSensorData gpsData GPSVelocitySensorData gpsVelData if !GPSPositionSensorReadOnly if !GPSVelocitySensorReadOnly GPSPositionSensorGet &gpsData GPSVelocitySensorGet &gpsVelData GPSPositionSensorData gpsPosition GPSPositionSensorGet &gpsPosition static int32_t getNED GPSPositionSensorData *gpsPosition float *NED
LibrePilot 12123 attitude.c Control 697874b315 Port of OP-754 On Revo. Fixed signs on bias calculation. Added rate zeroing at startup to yaw gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate gyrosBias.z -= - gyrosData.z * rollPitchBiasRate gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi - gyrosData.x - gyrosBias.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi - gyrosData.y - gyrosBias.y * rollPitchBiasRate gyrosBias.z -= - gyrosData.z - gyrosBias.z * rollPitchBiasRate
LibrePilot 12126 attitude.c Non-Control 711f1ad35b Fix indentation accel_filter_enabled = false rollPitchBiasRate = 0.01f accel_filter_enabled = false rollPitchBiasRate = 0.01f if accel_alpha > 0.0f init = 1 accel_mag = accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 accel_mag = sqrtf accel_mag grot_mag = sqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 else grot_mag = 1.0f accel_err 0 /= accel_mag * grot_mag accel_err 1 /= accel_mag * grot_mag accel_err 2 /= accel_mag * grot_mag if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE gyrosBias.z -= -gyrosData.z * rollPitchBiasRate gyrosBias.z -= - gyrosData.z - gyrosBias.z * rollPitchBiasRate
LibrePilot 12154 attitude.c Control a286a20cb0 OP-925 Changed dT to comply with revo sensor rate+review OPReview-484 const float fakeDt = 0.0015f
LibrePilot 12156 attitude.c Control 6435604182 some fixes and corrections as suggested in review else MagStateGet &magData gyroState.x = RAD2DEG gyros 0 - RAD2DEG Nav.gyro_bias 0 gyroState.y = RAD2DEG gyros 1 - RAD2DEG Nav.gyro_bias 1 gyroState.z = RAD2DEG gyros 2 - RAD2DEG Nav.gyro_bias 2
LibrePilot 12165 attitude.c Non-Control 23b2907d08 OP-976: Add the ; backThis compile successfuly with make all_flight MODULE_INITCALL AttitudeInitialize AttitudeStart MODULE_INITCALL AttitudeInitialize AttitudeStart
LibrePilot 12166 attitude.c Non-Control 23b2907d08 OP-976: Add the ; backThis compile successfuly with make all_flight MODULE_INITCALL AttitudeInitialize AttitudeStart MODULE_INITCALL AttitudeInitialize AttitudeStart
LibrePilot 12210 attitude.c Control c0b926a2d5 OP-990 zeroed the rollpitch bias rate at startup and incremented at arming. rollPitchBiasRate = 0.0f rollPitchBiasRate = 0.015f
LibrePilot 12220 attitude.c Control 3ad123718e Revert OP-990 zeroed the rollpitch bias rate at startup and incremented at arming. This reverts commit c0b926a2d5244dc9b64081273f93484ad6d4d281. rollPitchBiasRate = 0.01f rollPitchBiasRate = 0.01f
LibrePilot 12221 attitude.c Control 7a6b5c5681 OP-990 Fixed the main cause of wrong bias calculation:The initial bias calculation done between 1 and 7 secondssince power on was completely skipped due to a wrong condition. static bool magCalibrated = true float magBias 3 RevoCalibrationmag_biasGet magBias if magBias 0 < 1e-6f && magBias 1 < 1e-6f && magBias 2 < 1e-6f magCalibrated = false magData.x = 100.0f magData.y = 0.0f magData.z = 0.0f if xTaskGetTickCount < 7000 && xTaskGetTickCount > 1000 attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f init = 0 attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f if revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_FALSE gyrosBias.x -= accel_err 0 * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate gyrosBias.y -= accel_err 1 * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate gyrosBias.z -= -mag_err 2 * attitudeSettings.MagKi - gyrosData.z * rollPitchBiasRate
LibrePilot 12229 attitude.c Control 59eaf01089 OP-990 Change initial delay before calibration to 4 seconds to allow to put the craft on the ground after battery connection+review OPReview-505 if xTaskGetTickCount < 10000 && xTaskGetTickCount > 4000
LibrePilot 12251 attitude.c Control 49da1aca86 OP-1009 Redo gyro zero in Complementary after calibration parameters changes #define STACK_SIZE_BYTES 2048 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define FAILSAFE_TIMEOUT_MS 10 #define CALIBRATION_DELAY 4000 #define CALIBRATION_DURATION 6000 static uint32_t initStartupTime = 0 if xTaskGetTickCount < 3000 initStartupTime = 0 else initStartupTime = xTaskGetTickCount - CALIBRATION_DELAY GyrosBiasData zeroGyrosBias GyrosBiasGet &zeroGyrosBias zeroGyrosBias.x = 0 zeroGyrosBias.y = 0 zeroGyrosBias.z = 0 GyrosBiasSet &zeroGyrosBias if xTaskGetTickCount - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY && xTaskGetTickCount - initStartupTime > CALIBRATION_DELAY if !init AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_ERROR else if variance_error
LibrePilot 12255 attitude.c Non-Control d5a77fb525 OP-1009 prevent gyro zeroing from raise an alarm when arming if !init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED
LibrePilot 12258 attitude.c Non-Control b012d55f26 OP-1009 fix comments and fixes review+review OPReview-516
LibrePilot 12265 attitude.c Non-Control 0f5e354f99 Removed first occurance of accel_err scaling. We don t want to do thistwice.
LibrePilot 12266 attitude.c Non-Control e5b68382e0 Keep TODO comment.
LibrePilot 12270 attitude.c Non-Control 3392622d2b backporting change on next to modified Attitude module gyro_bias 0 = 0.0f gyro_bias 1 = 0.0f gyro_bias 2 = 0.0f
LibrePilot 12460 attitude.c Non-Control df90a13558 OP-1058: fix needed for fw_coptercontrol code compilation accelbias 0 = attitudeSettings.AccelBias.fields.X accelbias 1 = attitudeSettings.AccelBias.fields.Y accelbias 2 = attitudeSettings.AccelBias.fields.Z gyro_correct_int 0 = attitudeSettings.GyroBias.fields.X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias.fields.Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias.fields.Z / 100.0f if attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Roll == 0 && attitudeSettings.BoardRotation.fields.Yaw == 0 const float rpy 3 = attitudeSettings.BoardRotation.fields.Roll attitudeSettings.BoardRotation.fields.Pitch attitudeSettings.BoardRotation.fields.Yaw attitudeSettings.AccelBias.fields.X = trim_accels 0 / trim_samples attitudeSettings.AccelBias.fields.Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias.fields.Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE
LibrePilot 12463 attitude.c Non-Control 8351e97faa OP-1058: fix needed for fw_revoproto code compilation if invalid_var ekfConfiguration.R.fields.GPSPosNorth || invalid_var ekfConfiguration.R.fields.GPSPosEast || invalid_var ekfConfiguration.R.fields.GPSPosDown || invalid_var ekfConfiguration.R.fields.GPSVelNorth || invalid_var ekfConfiguration.R.fields.GPSVelEast || invalid_var ekfConfiguration.R.fields.GPSVelDown INSSetMagVar float 3 ekfConfiguration.R.fields.MagX ekfConfiguration.R.fields.MagY ekfConfiguration.R.fields.MagZ INSSetAccelVar float 3 ekfConfiguration.Q.fields.AccelX ekfConfiguration.Q.fields.AccelY ekfConfiguration.Q.fields.AccelZ INSSetGyroVar float 3 ekfConfiguration.Q.fields.GyroX ekfConfiguration.Q.fields.GyroY ekfConfiguration.Q.fields.GyroZ INSSetGyroBiasVar float 3 ekfConfiguration.Q.fields.GyroDriftX ekfConfiguration.Q.fields.GyroDriftY ekfConfiguration.Q.fields.GyroDriftZ INSSetBaroVar ekfConfiguration.R.fields.BaroZ INSResetP ekfConfiguration.P.data INSSetPosVelVar float 3 ekfConfiguration.R.fields.GPSPosNorth ekfConfiguration.R.fields.GPSPosEast ekfConfiguration.R.fields.GPSPosDown float 3 ekfConfiguration.R.fields.GPSVelNorth ekfConfiguration.R.fields.GPSVelEast ekfConfiguration.R.fields.GPSVelDown INSSetPosVelVar float 3 ekfConfiguration.FakeR.fields.FakeGPSPosIndoor ekfConfiguration.FakeR.fields.FakeGPSPosIndoor ekfConfiguration.FakeR.fields.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.fields.FakeGPSVelIndoor ekfConfiguration.FakeR.fields.FakeGPSVelIndoor ekfConfiguration.FakeR.fields.FakeGPSVelIndoor INSSetPosVelVar float 3 ekfConfiguration.FakeR.fields.FakeGPSPosIndoor ekfConfiguration.FakeR.fields.FakeGPSPosIndoor ekfConfiguration.FakeR.fields.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed INSGetP vardata.P.data if invalid_var ekfConfiguration.P.data t if invalid_var ekfConfiguration.Q.data t if invalid_var ekfConfiguration.R.data t
LibrePilot 12484 attitude.c Control e91bc28667 OP-1058 Get rid of Unions.They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions.It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays+review OPReview-552 accelbias 0 = attitudeSettings.AccelBias.X accelbias 1 = attitudeSettings.AccelBias.Y accelbias 2 = attitudeSettings.AccelBias.Z gyro_correct_int 0 = attitudeSettings.GyroBias.X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias.Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias.Z / 100.0f if attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Yaw == 0 const float rpy 3 = attitudeSettings.BoardRotation.Roll attitudeSettings.BoardRotation.Pitch attitudeSettings.BoardRotation.Yaw attitudeSettings.AccelBias.X = trim_accels 0 / trim_samples attitudeSettings.AccelBias.Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias.Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE #include if invalid_var ekfConfiguration.R.GPSPosNorth || invalid_var ekfConfiguration.R.GPSPosEast || invalid_var ekfConfiguration.R.GPSPosDown || invalid_var ekfConfiguration.R.GPSVelNorth || invalid_var ekfConfiguration.R.GPSVelEast || invalid_var ekfConfiguration.R.GPSVelDown INSSetMagVar float 3 ekfConfiguration.R.MagX ekfConfiguration.R.MagY ekfConfiguration.R.MagZ INSSetAccelVar float 3 ekfConfiguration.Q.AccelX ekfConfiguration.Q.AccelY ekfConfiguration.Q.AccelZ INSSetGyroVar float 3 ekfConfiguration.Q.GyroX ekfConfiguration.Q.GyroY ekfConfiguration.Q.GyroZ INSSetGyroBiasVar float 3 ekfConfiguration.Q.GyroDriftX ekfConfiguration.Q.GyroDriftY ekfConfiguration.Q.GyroDriftZ INSSetBaroVar ekfConfiguration.R.BaroZ INSResetP cast_struct_to_array ekfConfiguration.P ekfConfiguration.P.AttitudeQ1 INSSetPosVelVar float 3 ekfConfiguration.R.GPSPosNorth ekfConfiguration.R.GPSPosEast ekfConfiguration.R.GPSPosDown float 3 ekfConfiguration.R.GPSVelNorth ekfConfiguration.R.GPSVelEast ekfConfiguration.R.GPSVelDown INSSetPosVelVar float 3 ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.FakeGPSVelIndoor ekfConfiguration.FakeR.FakeGPSVelIndoor ekfConfiguration.FakeR.FakeGPSVelIndoor INSSetPosVelVar float 3 ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.FakeGPSVelAirspeed ekfConfiguration.FakeR.FakeGPSVelAirspeed ekfConfiguration.FakeR.FakeGPSVelAirspeed INSGetP cast_struct_to_array vardata.P vardata.P.AttitudeQ1 if invalid_var cast_struct_to_array ekfConfiguration.P ekfConfiguration.P.AttitudeQ1 t if invalid_var cast_struct_to_array ekfConfiguration.Q ekfConfiguration.Q.AccelX t if invalid_var cast_struct_to_array ekfConfiguration.R ekfConfiguration.R.BaroZ t
LibrePilot 12485 attitude.c Control e91bc28667 OP-1058 Get rid of Unions.They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions.It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays+review OPReview-552 accelbias 0 = attitudeSettings.AccelBias.X accelbias 1 = attitudeSettings.AccelBias.Y accelbias 2 = attitudeSettings.AccelBias.Z gyro_correct_int 0 = attitudeSettings.GyroBias.X / 100.0f gyro_correct_int 1 = attitudeSettings.GyroBias.Y / 100.0f gyro_correct_int 2 = attitudeSettings.GyroBias.Z / 100.0f if attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Yaw == 0 const float rpy 3 = attitudeSettings.BoardRotation.Roll attitudeSettings.BoardRotation.Pitch attitudeSettings.BoardRotation.Yaw attitudeSettings.AccelBias.X = trim_accels 0 / trim_samples attitudeSettings.AccelBias.Y = trim_accels 1 / trim_samples attitudeSettings.AccelBias.Z = trim_accels 2 / trim_samples + GRAV / ACCEL_SCALE #include if invalid_var ekfConfiguration.R.GPSPosNorth || invalid_var ekfConfiguration.R.GPSPosEast || invalid_var ekfConfiguration.R.GPSPosDown || invalid_var ekfConfiguration.R.GPSVelNorth || invalid_var ekfConfiguration.R.GPSVelEast || invalid_var ekfConfiguration.R.GPSVelDown INSSetMagVar float 3 ekfConfiguration.R.MagX ekfConfiguration.R.MagY ekfConfiguration.R.MagZ INSSetAccelVar float 3 ekfConfiguration.Q.AccelX ekfConfiguration.Q.AccelY ekfConfiguration.Q.AccelZ INSSetGyroVar float 3 ekfConfiguration.Q.GyroX ekfConfiguration.Q.GyroY ekfConfiguration.Q.GyroZ INSSetGyroBiasVar float 3 ekfConfiguration.Q.GyroDriftX ekfConfiguration.Q.GyroDriftY ekfConfiguration.Q.GyroDriftZ INSSetBaroVar ekfConfiguration.R.BaroZ INSResetP cast_struct_to_array ekfConfiguration.P ekfConfiguration.P.AttitudeQ1 INSSetPosVelVar float 3 ekfConfiguration.R.GPSPosNorth ekfConfiguration.R.GPSPosEast ekfConfiguration.R.GPSPosDown float 3 ekfConfiguration.R.GPSVelNorth ekfConfiguration.R.GPSVelEast ekfConfiguration.R.GPSVelDown INSSetPosVelVar float 3 ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.FakeGPSVelIndoor ekfConfiguration.FakeR.FakeGPSVelIndoor ekfConfiguration.FakeR.FakeGPSVelIndoor INSSetPosVelVar float 3 ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor ekfConfiguration.FakeR.FakeGPSPosIndoor float 3 ekfConfiguration.FakeR.FakeGPSVelAirspeed ekfConfiguration.FakeR.FakeGPSVelAirspeed ekfConfiguration.FakeR.FakeGPSVelAirspeed INSGetP cast_struct_to_array vardata.P vardata.P.AttitudeQ1 if invalid_var cast_struct_to_array ekfConfiguration.P ekfConfiguration.P.AttitudeQ1 t if invalid_var cast_struct_to_array ekfConfiguration.Q ekfConfiguration.Q.AccelX t if invalid_var cast_struct_to_array ekfConfiguration.R ekfConfiguration.R.BaroZ t
LibrePilot 12503 attitude.c Non-Control 0db9a9bf8d OP-1058 Add xxxGet and xxxSet functions to handle multielement fields as structfor example:EKFStateVariancePSet(EKFStateVariancePData *NewP);EKFStateVariancePGet(EKFStateVariancePData *NewP);Also in this case array accessors are renamed as xxxArrayGet/Set:EKFStateVariancePArraySet(float *NewP);EKFStateVariancePArrayGet(float *NewP);Nothing changes for anonymous items as default functions continues to deal with arrays+review OPReview-552 attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL RevoCalibrationmag_biasArrayGet magBias
LibrePilot 12504 attitude.c Non-Control 0db9a9bf8d OP-1058 Add xxxGet and xxxSet functions to handle multielement fields as structfor example:EKFStateVariancePSet(EKFStateVariancePData *NewP);EKFStateVariancePGet(EKFStateVariancePData *NewP);Also in this case array accessors are renamed as xxxArrayGet/Set:EKFStateVariancePArraySet(float *NewP);EKFStateVariancePArrayGet(float *NewP);Nothing changes for anonymous items as default functions continues to deal with arrays+review OPReview-552 attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL RevoCalibrationmag_biasArrayGet magBias
LibrePilot 12645 attitude.c Control 260db3446c OP-1149 changes in CC/CC3D code to use AccelGyroSettings for bias/scale instead of AttitudeSettings.Bias units of measure are now normalized between CC/CC3D/Revo. #include accelgyrosettings.h bool apply_gyro_temp = false bool apply_accel_temp = false static float gyro_temp_coeff 4 = 0 static float accel_temp_coeff 4 = 0 static float gyro_scale 3 = 0 static float accel_scale 3 = 0 #define STD_CC_ACCEL_SCALE GRAV * 0.004f #define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_GAIN 0.42f static const struct pios_board_info *bdinfo = &pios_board_info_blob #define BOARDISCC3D bdinfo->board_rev == 0x02 AccelGyroSettingsInitialize AccelGyroSettingsConnectCallback &settingsUpdatedCb bool cc3d = BOARDISCC3D gyros->x = - gyro 1 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale 0 gyros->y = gyro 2 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale 1 gyros->z = - gyro 3 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale 2 float accel 3 = accel_scale 0 * float x / i accel_scale 1 * float y / i accel_scale 2 * float z / i accelState->x -= accelbias 0 accelState->y -= accelbias 1 accelState->z -= accelbias 2 gyros 0 = mpu6000_data.gyro_x * gyro_scale 0 gyros 1 = mpu6000_data.gyro_y * gyro_scale 1 gyros 2 = mpu6000_data.gyro_z * gyro_scale 2 accels 0 = mpu6000_data.accel_x * accel_scale 0 accels 1 = mpu6000_data.accel_y * accel_scale 1 accels 2 = mpu6000_data.accel_z * accel_scale 2 accelStateData->x = accels 0 - accelbias 0 accelStateData->y = accels 1 - accelbias 1 accelStateData->z = accels 2 - accelbias 2 AccelGyroSettingsData accelGyroSettings AccelGyroSettingsGet &accelGyroSettings gyro_temp_coeff 0 = accelGyroSettings.gyro_temp_coeff.X gyro_temp_coeff 1 = accelGyroSettings.gyro_temp_coeff.Y gyro_temp_coeff 2 = accelGyroSettings.gyro_temp_coeff.Z gyro_temp_coeff 3 = accelGyroSettings.gyro_temp_coeff.Z2 accel_temp_coeff 0 = accelGyroSettings.accel_temp_coeff.X accel_temp_coeff 1 = accelGyroSettings.accel_temp_coeff.Y accel_temp_coeff 2 = accelGyroSettings.accel_temp_coeff.Z apply_gyro_temp = fabsf gyro_temp_coeff 0 > 1e-6f || fabsf gyro_temp_coeff 1 > 1e-6f || fabsf gyro_temp_coeff 2 > 1e-6f || fabsf gyro_temp_coeff 3 > 1e-6f apply_accel_temp = fabsf accel_temp_coeff 0 > 1e-6f || fabsf accel_temp_coeff 1 > 1e-6f || fabsf accel_temp_coeff 2 > 1e-6f gyro_correct_int 0 = accelGyroSettings.gyro_bias.X gyro_correct_int 1 = accelGyroSettings.gyro_bias.Y gyro_correct_int 2 = accelGyroSettings.gyro_bias.Z if BOARDISCC3D accelbias 0 = accelGyroSettings.accel_bias.X accelbias 1 = accelGyroSettings.accel_bias.Y accelbias 2 = accelGyroSettings.accel_bias.Z gyro_scale 0 = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale gyro_scale 1 = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale gyro_scale 2 = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale accel_scale 0 = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale accel_scale 1 = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale accel_scale 2 = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale else accelbias 0 = accelGyroSettings.accel_bias.X accelbias 1 = accelGyroSettings.accel_bias.Y accelbias 2 = accelGyroSettings.accel_bias.Z gyro_scale 0 = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN gyro_scale 1 = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN gyro_scale 2 = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN accel_scale 0 = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE accel_scale 1 = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE accel_scale 2 = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE accelGyroSettings.accel_scale.X = trim_accels 0 / trim_samples accelGyroSettings.accel_scale.Y = trim_accels 1 / trim_samples accelGyroSettings.accel_scale.Z = trim_accels 2 / trim_samples + GRAV
LibrePilot 12646 attitude.c Control ac3d7b8d11 OP-1149 Implements thermal accel/gyro bias correction for CC3D static bool apply_gyro_temp = false static bool apply_accel_temp = false static struct pios_mpu6000_data mpu6000_data if apply_gyro_temp gyros 0 -= gyro_temp_coeff 0 * mpu6000_data.temperature gyros 1 -= gyro_temp_coeff 1 * mpu6000_data.temperature gyros 2 -= gyro_temp_coeff 2 + gyro_temp_coeff 3 * mpu6000_data.temperature * mpu6000_data.temperature if apply_accel_temp accels 0 -= accel_temp_coeff 0 * mpu6000_data.temperature accels 1 -= accel_temp_coeff 1 * mpu6000_data.temperature accels 2 -= accel_temp_coeff 2 * mpu6000_data.temperature
LibrePilot 12647 attitude.c Non-Control e04cef2fa4 OP-1149 Uncrustify static bool apply_gyro_temp = false static bool apply_accel_temp = false static float gyro_temp_coeff 4 = 0 static float accel_temp_coeff 4 = 0 static float gyro_scale 3 = 0 static float accel_scale 3 = 0 #define GRAV 9.81f #define STD_CC_ACCEL_SCALE GRAV * 0.004f #define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_GAIN 0.42f #define BOARDISCC3D bdinfo->board_rev == 0x02 float accel 3 = accel_scale 0 * float x / i accel_scale 1 * float y / i accel_scale 2 * float z / i if apply_gyro_temp gyros 0 -= gyro_temp_coeff 0 * mpu6000_data.temperature gyros 1 -= gyro_temp_coeff 1 * mpu6000_data.temperature gyros 2 -= gyro_temp_coeff 2 + gyro_temp_coeff 3 * mpu6000_data.temperature * mpu6000_data.temperature if apply_accel_temp zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE gyro_temp_coeff 0 = accelGyroSettings.gyro_temp_coeff.X gyro_temp_coeff 1 = accelGyroSettings.gyro_temp_coeff.Y gyro_temp_coeff 2 = accelGyroSettings.gyro_temp_coeff.Z gyro_temp_coeff 3 = accelGyroSettings.gyro_temp_coeff.Z2 apply_gyro_temp = fabsf gyro_temp_coeff 0 > 1e-6f || fabsf gyro_temp_coeff 1 > 1e-6f || fabsf gyro_temp_coeff 2 > 1e-6f || fabsf gyro_temp_coeff 3 > 1e-6f apply_accel_temp = fabsf accel_temp_coeff 0 > 1e-6f || fabsf accel_temp_coeff 1 > 1e-6f || fabsf accel_temp_coeff 2 > 1e-6f if BOARDISCC3D accelbias 0 = accelGyroSettings.accel_bias.X accelbias 1 = accelGyroSettings.accel_bias.Y accelbias 2 = accelGyroSettings.accel_bias.Z gyro_scale 0 = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale gyro_scale 1 = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale gyro_scale 2 = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale accelbias 0 = accelGyroSettings.accel_bias.X accelbias 1 = accelGyroSettings.accel_bias.Y accelbias 2 = accelGyroSettings.accel_bias.Z gyro_scale 0 = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN gyro_scale 1 = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN gyro_scale 2 = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL
LibrePilot 12759 attitude.c Control eea194d22f OP-1149: fix type for accel bias. static float accel_bias 3 accelState->x -= accel_bias 0 accelState->y -= accel_bias 1 accelState->z -= accel_bias 2 accelStateData->x = accels 0 - accel_bias 0 accelStateData->y = accels 1 - accel_bias 1 accelStateData->z = accels 2 - accel_bias 2 accel_bias 0 = accelGyroSettings.accel_bias.X accel_bias 1 = accelGyroSettings.accel_bias.Y accel_bias 2 = accelGyroSettings.accel_bias.Z accel_bias 0 = accelGyroSettings.accel_bias.X accel_bias 1 = accelGyroSettings.accel_bias.Y accel_bias 2 = accelGyroSettings.accel_bias.Z
LibrePilot 12764 attitude.c Control 28be9cc8ce OP-1211 adapted existing code to use new pios functionality instead of separate error prone implementations #define UPDATE_EXPECTED 1.0f / 666.0f #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f static PiOSDeltatimeConfig dtconfig PIOS_DELTATIME_Init &dtconfig UPDATE_EXPECTED UPDATE_MIN UPDATE_MAX UPDATE_ALPHA float dT = PIOS_DELTATIME_GetAverageSeconds &dtconfig
LibrePilot 12772 attitude.c Control 5bb5b34447 OP-1149: second order correction for all gyro axis. Add calibrated range contraint for temperature.cleanup replacing all the arrays of floats[] with corresponding AccelGyroSettings* structs. static AccelGyroSettingsaccel_biasData accel_bias static bool apply_gyro_temp = false static bool apply_accel_temp = false static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent static AccelGyroSettingsgyro_scaleData gyro_scale static AccelGyroSettingsaccel_scaleData accel_scale gyros->x = - gyro 1 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale.X gyros->y = gyro 2 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale.Y gyros->z = - gyro 3 - STD_CC_ANALOG_GYRO_NEUTRAL * gyro_scale.Z float accel 3 = accel_scale.X * float x / i accel_scale.Y * float y / i accel_scale.Z * float z / i accelState->x -= accel_bias.X accelState->y -= accel_bias.Y accelState->z -= accel_bias.Z gyros 0 = mpu6000_data.gyro_x * gyro_scale.X gyros 1 = mpu6000_data.gyro_y * gyro_scale.Y gyros 2 = mpu6000_data.gyro_z * gyro_scale.Z accels 0 = mpu6000_data.accel_x * accel_scale.X accels 1 = mpu6000_data.accel_y * accel_scale.Y accels 2 = mpu6000_data.accel_z * accel_scale.Z float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max : mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min : mpu6000_data.temperature gyros 0 -= gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp * ctemp gyros 1 -= gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp * ctemp gyros 2 -= gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp * ctemp accels 0 -= accel_temp_coeff.X * ctemp accels 1 -= accel_temp_coeff.Y * ctemp accels 2 -= accel_temp_coeff.Z * ctemp accelStateData->x = accels 0 - accel_bias.X accelStateData->y = accels 1 - accel_bias.Y accelStateData->z = accels 2 - accel_bias.Z zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE memcpy &gyro_temp_coeff &accelGyroSettings.gyro_temp_coeff sizeof AccelGyroSettingsgyro_temp_coeffData memcpy &accel_temp_coeff &accelGyroSettings.accel_temp_coeff sizeof AccelGyroSettingsaccel_temp_coeffData apply_gyro_temp = fabsf gyro_temp_coeff.X > 1e-6f || fabsf gyro_temp_coeff.Y > 1e-6f || fabsf gyro_temp_coeff.Z > 1e-6f || fabsf gyro_temp_coeff.X2 > 1e-6f || fabsf gyro_temp_coeff.Y2 > 1e-6f || fabsf gyro_temp_coeff.Z2 > 1e-6f apply_accel_temp = fabsf accel_temp_coeff.X > 1e-6f || fabsf accel_temp_coeff.Y > 1e-6f || fabsf accel_temp_coeff.Z > 1e-6f temp_calibrated_extent.min = accelGyroSettings.temp_calibrated_extent.min temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max accel_bias.X = accelGyroSettings.accel_bias.X accel_bias.Y = accelGyroSettings.accel_bias.Y accel_bias.Z = accelGyroSettings.accel_bias.Z gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale accel_bias.X = accelGyroSettings.accel_bias.X accel_bias.Y = accelGyroSettings.accel_bias.Y accel_bias.Z = accelGyroSettings.accel_bias.Z gyro_scale.X = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN gyro_scale.Y = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN gyro_scale.Z = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN accel_scale.X = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE accel_scale.Y = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE accel_scale.Z = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE
LibrePilot 12892 attitude.c Control 9d30673d18 OP-1273 attitude estimation uses airspeedSensor.TrueAirspeed when available (e.g. from MS4525DO; available when TrueAirspeed >=0 ) instead of using an altitude correction IAS2TAS(alt). When not available it falls back to IAS2TAS(alt) airspeed.TrueAirspeed = airspeedSensor.TrueAirspeed < 0.f ? airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionState.Down : airspeedSensor.TrueAirspeed airspeed.TrueAirspeed = airspeedSensor.TrueAirspeed < 0.f ? airspeed.CalibratedAirspeed * IAS2TAS homeLocation.Altitude - positionState.Down : airspeedSensor.TrueAirspeed
LibrePilot 12903 attitude.c Non-Control c5dc556a43 OP-1273 Uncrustify airspeed.TrueAirspeed = airspeedSensor.TrueAirspeed < 0.f ? airspeed.CalibratedAirspeed *IAS2TAS homeLocation.Altitude - positionState.Down : airspeedSensor.TrueAirspeed airspeed.TrueAirspeed = airspeedSensor.TrueAirspeed < 0.f ? airspeed.CalibratedAirspeed *IAS2TAS homeLocation.Altitude - positionState.Down : airspeedSensor.TrueAirspeed
LibrePilot 12928 attitude.c Non-Control fd5d1e42a3 OP-1273 OP-1282 OP-1283 corrected compilation error for fw_revoproto PositionStateData positionState PositionStateGet &positionState positionState.North = Nav.Pos 0 positionState.East = Nav.Pos 1 positionState.Down = Nav.Pos 2 PositionStateSet &positionState airspeed.TrueAirspeed = airspeedData.TrueAirspeed < 0.f ? airspeed.CalibratedAirspeed *IAS2TAS homeLocation.Altitude - positionState.Down : airspeedData.TrueAirspeed
LibrePilot 12945 attitude.c Control 011c598793 OP-1296 only use GPS in filterchains when explicitly requesteddisallow arming when GPS is needed for initialization until lock is acquired case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
LibrePilot 12952 attitude.c Control 1fe591d51e OP-975 change BoardRotation data type to float so it can be used for level calibration if fabsf attitudeSettings.BoardRotation.Pitch < 0.00001f && fabsf attitudeSettings.BoardRotation.Roll < 0.00001f && fabsf attitudeSettings.BoardRotation.Yaw < 0.00001f
LibrePilot 12969 attitude.c Non-Control 20508314f2 OP-1302 use pios_notify api to alert user of ongoing CF gyro calibration #include PIOS_NOTIFY_StartNotification NOTIFY_DRAW_ATTENTION NOTIFY_PRIORITY_REGULAR PIOS_NOTIFY_StartNotification NOTIFY_DRAW_ATTENTION NOTIFY_PRIORITY_REGULAR
LibrePilot 13164 attitude.c Non-Control be5cc66bd3 OP-1274 fixes for Coptercontrol firmware xTaskCreate AttitudeTask const char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle
LibrePilot 13186 attitude.c Non-Control 3ec8c18fad OP-1274 fixes for RevoProto xTaskCreate AttitudeTask const char * Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle
LibrePilot 13234 attitude.c Control d51b7ce2d4 OP-1227 Fix high CPU usage: Changes to attitude module:- changed Expected rate to 500 (it was erroneously defined as 666Hz)- use the optimized api to fetch accel samples #undef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION #include int8_t counterUpd int8_t counterAccelSamples = -1 #endif #define UPDATE_EXPECTED 1.0f / 500.0f #ifdef PIOS_INCLUDE_INSTRUMENTATION counterUpd = PIOS_Instrumentation_CreateCounter 0xA7710001 int8_t counterAtt = PIOS_Instrumentation_CreateCounter 0xA7710002 int8_t counterPeriod = PIOS_Instrumentation_CreateCounter 0xA7710003 if !cc3d counterAccelSamples = PIOS_Instrumentation_CreateCounter 0xA7710004 #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeStart counterAtt #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeEnd counterAtt #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TrackPeriod counterPeriod #endif uint8_t fifoSamples = PIOS_ADXL345_FifoElements if fifoSamples == 0 #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeStart counterUpd #endif uint8_t i = fifoSamples uint8_t samples_remaining samples_remaining = PIOS_ADXL345_ReadAndAccumulateSamples &accel_data fifoSamples x = accel_data.x y = -accel_data.y z = -accel_data.z if samples_remaining > 0 do i++ samples_remaining = PIOS_ADXL345_Read &accel_data x += accel_data.x y += -accel_data.y z += -accel_data.z while i < 32 && samples_remaining > 0 #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_updateCounter counterAccelSamples i #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeEnd counterUpd #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeStart counterUpd vPortEnterCritical #endif #ifdef PIOS_INCLUDE_INSTRUMENTATION vPortExitCritical PIOS_Instrumentation_TimeEnd counterUpd #endif
LibrePilot 13235 attitude.c Control 843987484b OP-1227 Fix high CPU usage: Avoid several float division in Attitude module change optimization for attitude estimation function __attribute__ optimize O3 static void updateAttitude AccelStateData *accelStateData GyroStateData *gyrosData const float invMag = 1.0f / accel_mag * grot_mag accel_err 0 *= invMag accel_err 1 *= invMag accel_err 2 *= invMag const float kpInvdT = accelKp / dT gyros 0 += accel_err 0 * kpInvdT gyros 1 += accel_err 1 * kpInvdT gyros 2 += accel_err 2 * kpInvdT const float invQmag = 1.0f / qmag q 0 = q 0 * invQmag q 1 = q 1 * invQmag q 2 = q 2 * invQmag q 3 = q 3 * invQmag
LibrePilot 13237 attitude.c Control 52c1d69c19 OP-1227 Fix high CPU usage: Change gyro oversampling to 46 (400Hz update rate) to lessen the cpu load and jitter due accel rate mismatch PIOS_ADC_Config 46
LibrePilot 13259 attitude.c Control 16cb081f5c OP-1365 Flight code instrumentation API: Sample use of instrumentation in Atittude module.When instrumentation is enabled at board level (pios_board.h enable PIOS_INCLUDE_INSTRUMENTATION) PIOS_INSTRUMENT_MODULE setups the module to be instrumented. #include PERF_DEFINE_COUNTER counterUpd PERF_DEFINE_COUNTER counterAccelSamples PERF_DEFINE_COUNTER counterPeriod PERF_DEFINE_COUNTER counterAtt PERF_INIT_COUNTER counterUpd 0xA7710001 PERF_INIT_COUNTER counterAtt 0xA7710002 PERF_INIT_COUNTER counterPeriod 0xA7710003 PERF_INIT_COUNTER counterAccelSamples 0xA7710004 PERF_TIMED_SECTION_START counterAtt PERF_TIMED_SECTION_END counterAtt PERF_MEASURE_PERIOD counterPeriod PERF_TIMED_SECTION_START counterUpd PERF_TRACK_VALUE counterAccelSamples i PERF_TIMED_SECTION_END counterUpd PERF_TIMED_SECTION_START counterUpd PERF_TIMED_SECTION_END counterUpd
LibrePilot 13340 attitude.c Non-Control eb5deb3eca OP-1274 Remove unneeded cast from task name in xTaskCreate call xTaskCreate AttitudeTask Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle xTaskCreate AttitudeTask Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle
LibrePilot 13341 attitude.c Non-Control eb5deb3eca OP-1274 Remove unneeded cast from task name in xTaskCreate call xTaskCreate AttitudeTask Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &taskHandle xTaskCreate AttitudeTask Attitude STACK_SIZE_BYTES / 4 NULL TASK_PRIORITY &attitudeTaskHandle
LibrePilot 13409 attitude.c Non-Control 57a43a5587 OP-1403 - Migrate to new HMC5x83 driver and remove old HMC5883 one #if defined PIOS_INCLUDE_HMC5X83
LibrePilot 13422 attitude.c Non-Control 016ba6940d OP-1403 - drop attitude/revolution as it has been supersede by StateEstimation module
LibrePilot 13767 attitude.c Control c1641613ce OP-1515 - Attitude module was not reading all gyro/accel fifo items causing it to lag behind by at least a sample float accels 3 = 0 float gyros 3 = 0 float temp = 0 uint8_t count = 0 gyros 0 += mpu6000_data.gyro_x gyros 1 += mpu6000_data.gyro_y gyros 2 += mpu6000_data.gyro_z accels 0 += mpu6000_data.accel_x accels 1 += mpu6000_data.accel_y accels 2 += mpu6000_data.accel_z temp += mpu6000_data.temperature count++ if !count float invcount = 1.0f / count gyros 0 *= gyro_scale.X * invcount gyros 1 *= gyro_scale.Y * invcount gyros 2 *= gyro_scale.Z * invcount accels 0 *= accel_scale.X * invcount accels 1 *= accel_scale.Y * invcount accels 2 *= accel_scale.Z * invcount temp *= invcount float ctemp = temp > temp_calibrated_extent.max ? temp_calibrated_extent.max : temp < temp_calibrated_extent.min ? temp_calibrated_extent.min : temp
LibrePilot 13774 attitude.c Control a5fd040b49 OP-1515 - Attitude module was not reading all gyro/accel fifo items causing it to lag behind by at least a sample float accels 3 = 0 float gyros 3 = 0 float temp = 0 uint8_t count = 0 BaseType_t ret = xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD while ret == pdTRUE gyros 0 += mpu6000_data.gyro_x gyros 1 += mpu6000_data.gyro_y gyros 2 += mpu6000_data.gyro_z accels 0 += mpu6000_data.accel_x accels 1 += mpu6000_data.accel_y accels 2 += mpu6000_data.accel_z temp += mpu6000_data.temperature count++ ret = xQueueReceive queue void * &mpu6000_data 0 if !count float invcount = 1.0f / count gyros 0 *= gyro_scale.X * invcount gyros 1 *= gyro_scale.Y * invcount gyros 2 *= gyro_scale.Z * invcount accels 0 *= accel_scale.X * invcount accels 1 *= accel_scale.Y * invcount accels 2 *= accel_scale.Z * invcount temp *= invcount float ctemp = temp > temp_calibrated_extent.max ? temp_calibrated_extent.max : temp < temp_calibrated_extent.min ? temp_calibrated_extent.min : temp
LibrePilot 13781 attitude.c Control 55402880bc Revert Merge remote-tracking branch origin/amorale/OP-1515_fix_attitude_fifo into next This reverts commit 316f7fcf026d912484827a2de2dae3000979f327 reversingchanges made to cfa0ef4671829f7834fa83d390cee80858ba1919. float accels 3 gyros 3 gyros 0 = mpu6000_data.gyro_x * gyro_scale.X gyros 1 = mpu6000_data.gyro_y * gyro_scale.Y gyros 2 = mpu6000_data.gyro_z * gyro_scale.Z accels 0 = mpu6000_data.accel_x * accel_scale.X accels 1 = mpu6000_data.accel_y * accel_scale.Y accels 2 = mpu6000_data.accel_z * accel_scale.Z float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max : mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min : mpu6000_data.temperature
LibrePilot 13829 attitude.c Control 46fee43487 OP-1531 Filter temperatures for gyro and accel calibration. calculate new bias every 30 samples #include #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define SENSOR_PERIOD 4 #define UPDATE_RATE 25.0f #define TEMP_CALIB_INTERVAL 30 #define TEMP_ALPHA 0.9f #define UPDATE_EXPECTED 1.0f / 500.0f #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f static float temperature = 0 static float accel_temp_bias 3 = 0 static float gyro_temp_bias 3 = 0 static uint8_t temp_calibration_count = 0 gyros 0 *= gyro_scale.X * invcount gyros 1 *= gyro_scale.Y * invcount gyros 2 *= gyro_scale.Z * invcount accels 0 *= accel_scale.X * invcount accels 1 *= accel_scale.Y * invcount accels 2 *= accel_scale.Z * invcount temperature = TEMP_ALPHA * temperature + 1 - TEMP_ALPHA * temp if apply_gyro_temp || apply_accel_temp && !temp_calibration_count temp_calibration_count = TEMP_CALIB_INTERVAL float ctemp = boundf temp temp_calibrated_extent.max temp_calibrated_extent.min if apply_gyro_temp gyro_temp_bias 0 = gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp * ctemp gyro_temp_bias 1 = gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp * ctemp gyro_temp_bias 2 = gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp * ctemp if apply_accel_temp accel_temp_bias 0 = accel_temp_coeff.X * ctemp accel_temp_bias 1 = accel_temp_coeff.Y * ctemp accel_temp_bias 2 = accel_temp_coeff.Z * ctemp temp_calibration_count-- gyros 0 -= gyro_temp_bias 0 gyros 1 -= gyro_temp_bias 1 gyros 2 -= gyro_temp_bias 2 accels 0 -= accel_temp_bias 0 accels 1 -= accel_temp_bias 1 accels 2 -= accel_temp_bias 2
LibrePilot 13832 attitude.c Control 9350bd65d9 OP-1535 - change sensor rate to 1KHz with software downsample to 500HzAllow per board SPI prescaler settings #define SENSOR_PERIOD 2 while PIOS_ADXL345_FifoElements == 0 AlarmsSet SYSTEMALARMS_ALARM_ATTITUDE SYSTEMALARMS_ALARM_CRITICAL PIOS_WDG_UpdateFlag PIOS_WDG_ATTITUDE portTickType lastSysTime = xTaskGetTickCount vTaskDelayUntil &lastSysTime SENSOR_PERIOD / portTICK_RATE_MS BaseType_t ret = xQueueReceive queue void * &mpu6000_data 0
LibrePilot 13844 attitude.c Control 9499cf9abc OP-1535 - Some optimizations for attitude module #include #define STD_CC_ACCEL_SCALE PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f vTaskDelayUntil &lastSysTime SENSOR_PERIOD / portTICK_PERIOD_MS BaseType_t ret = xQueueReceive queue void * &mpu6000_data SENSOR_PERIOD PERF_TRACK_VALUE counterAccelSamples count float inv_accel_mag = fast_invsqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 if inv_accel_mag > 1e3f float inv_grot_mag inv_grot_mag = fast_invsqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 inv_grot_mag = 1.0f if inv_grot_mag > 1e3f const float invMag = inv_accel_mag * inv_grot_mag float inv_qmag = fast_invsqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3 if fabsf inv_qmag > 1e3f || isnan inv_qmag q 0 = q 0 * inv_qmag q 1 = q 1 * inv_qmag q 2 = q 2 * inv_qmag q 3 = q 3 * inv_qmag accelGyroSettings.accel_scale.Z = trim_accels 2 / trim_samples + PIOS_CONST_MKS_GRAV_ACCEL_F
LibrePilot 13847 attitude.c Control f2fc5f5616 OP-1513 - Fix time constants set initial temperature as first sample value #define TEMP_ALPHA 0.999504f static float temperature = NAN if isnan temperature temperature = temp float ctemp = boundf temperature temp_calibrated_extent.max temp_calibrated_extent.min
LibrePilot 13853 attitude.c Control 5d074063d1 OP-1535 - Add board specific sensor update rate constant static const uint32_t sensor_period_ms = uint32_t 1000.0f / PIOS_SENSOR_RATE #define TEMP_DT 1.0f / PIOS_SENSOR_RATE #define TEMP_LPF_FC 5.0f static const float temp_alpha = TEMP_DT / TEMP_DT + 1.0f / 2.0f * M_PI_F * TEMP_LPF_FC #define UPDATE_EXPECTED 1.0f / PIOS_SENSOR_RATE vTaskDelayUntil &lastSysTime sensor_period_ms / portTICK_PERIOD_MS BaseType_t ret = xQueueReceive queue void * &mpu6000_data sensor_period_ms gyros 0 *= gyro_scale.X * invcount gyros 1 *= gyro_scale.Y * invcount gyros 2 *= gyro_scale.Z * invcount accels 0 *= accel_scale.X * invcount accels 1 *= accel_scale.Y * invcount accels 2 *= accel_scale.Z * invcount if isnan temperature temperature = temp_alpha * temp - temperature + temperature
LibrePilot 14011 attitude.c Control 0cf5d163d1 OP-1658 - Fixes for CC3D and its attitude module #include #include #include static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL #define BOARDISCC3D bdinfo->board_rev == 0x02 gyro_test = PIOS_MPU6000_Driver.test 0 mpu6000_data = pios_malloc sizeof PIOS_SENSORS_3Axis_SensorsWithTemp + sizeof Vector3i16 * 2 xQueueHandle queue = PIOS_MPU6000_Driver.get_queue 0 BaseType_t ret = xQueueReceive queue void * mpu6000_data sensor_period_ms gyros 0 += mpu6000_data->sample 1 .x gyros 1 += mpu6000_data->sample 1 .y gyros 2 += mpu6000_data->sample 1 .z accels 0 += mpu6000_data->sample 0 .x accels 1 += mpu6000_data->sample 0 .y accels 2 += mpu6000_data->sample 0 .z temp += mpu6000_data->temperature float scales 2 PIOS_MPU6000_Driver.get_scale scales 2 0 gyro_scale.X = accelGyroSettings.gyro_scale.X * scales 0 gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales 0 gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales 0 accel_scale.X = accelGyroSettings.accel_scale.X * scales 1 accel_scale.Y = accelGyroSettings.accel_scale.Y * scales 1 accel_scale.Z = accelGyroSettings.accel_scale.Z * scales 1
LibrePilot 14052 attitude.c Non-Control 84465ae0cb OP-1743 - Fix a very ugly error in attitude(cc/cc3d) #ifdef PIOS_INCLUDE_WDG #endif #ifdef PIOS_INCLUDE_WDG #endif #ifdef PIOS_INCLUDE_WDG #endif ret = xQueueReceive queue void * mpu6000_data 0
LibrePilot 14056 attitude.c Control a01cab1e72 OP-1743 - Fix attitude issue on CC3D gyro_scale.X = accelGyroSettings.gyro_scale.X * scales 1 gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales 1 gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales 1 accel_scale.X = accelGyroSettings.accel_scale.X * scales 0 accel_scale.Y = accelGyroSettings.accel_scale.Y * scales 0 accel_scale.Z = accelGyroSettings.accel_scale.Z * scales 0
LibrePilot 14733 attitude.c Control 71a0a563f1 LP-204 Replace fast_invsqrtf() with invsqrtf() : 1.0f / sqrtf() float inv_accel_mag = invsqrtf accels_filtered 0 * accels_filtered 0 + accels_filtered 1 * accels_filtered 1 + accels_filtered 2 * accels_filtered 2 inv_grot_mag = invsqrtf grot_filtered 0 * grot_filtered 0 + grot_filtered 1 * grot_filtered 1 + grot_filtered 2 * grot_filtered 2 float inv_qmag = invsqrtf q 0 * q 0 + q 1 * q 1 + q 2 * q 2 + q 3 * q 3
LibrePilot 14920 attitude.c Control 6bd6ef5be1 LP-150 - Run attitude estimation at a configurable fraction of gyro sample rate for CC/CC3D static uint32_t samples = 0 static float gyros_accum 3 static float accels_accum 3 if samples < ATTITUDE_SENSORS_DOWNSAMPLE - 1 gyros_accum 0 += gyros 0 gyros_accum 1 += gyros 1 gyros_accum 2 += gyros 2 accels_accum 0 += accels 0 accels_accum 1 += accels 1 accels_accum 2 += accels 2 samples++ return float dT = PIOS_DELTATIME_GetAverageSeconds &dtconfig PERF_TIMED_SECTION_START counterAtt float inv_samples_count = 1.0f / float samples samples = 0 gyros_accum 0 *= inv_samples_count gyros_accum 1 *= inv_samples_count gyros_accum 2 *= inv_samples_count accels_accum 0 *= inv_samples_count accels_accum 1 *= inv_samples_count accels_accum 2 *= inv_samples_count apply_accel_filter accels_accum accels_filtered gyros_accum 0 += accel_err 0 * kpInvdT gyros_accum 1 += accel_err 1 * kpInvdT gyros_accum 2 += accel_err 2 * kpInvdT qdot 0 = -q 1 * gyros_accum 0 - q 2 * gyros_accum 1 - q 3 * gyros_accum 2 * dT * M_PI_F / 180.0f / 2.0f qdot 1 = q 0 * gyros_accum 0 - q 3 * gyros_accum 1 + q 2 * gyros_accum 2 * dT * M_PI_F / 180.0f / 2.0f qdot 2 = q 3 * gyros_accum 0 + q 0 * gyros_accum 1 - q 1 * gyros_accum 2 * dT * M_PI_F / 180.0f / 2.0f qdot 3 = -q 2 * gyros_accum 0 + q 1 * gyros_accum 1 + q 0 * gyros_accum 2 * dT * M_PI_F / 180.0f / 2.0f gyros_accum 0 = gyros_accum 1 = gyros_accum 2 = 0.0f accels_accum 0 = accels_accum 1 = accels_accum 2 = 0.0f PERF_TIMED_SECTION_END counterAtt PERF_MEASURE_PERIOD counterPeriod
LibrePilot 15063 attitude.c Control f42c33a922 LP-327 - CC/CC3D Implementation #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY tskIDLE_PRIORITY + 3 #define UPDATE_RATE 25.0f #define TEMP_CALIB_INTERVAL 30 #define TEMP_DT 1.0f / PIOS_SENSOR_RATE #define TEMP_LPF_FC 5.0f #define UPDATE_EXPECTED 1.0f / PIOS_SENSOR_RATE #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f #define VARIANCE_WINDOW_SIZE 40 static pw_variance_t gyro_var 3 static bool initialZeroWhenBoardSteady = true static float boardSteadyMaxVariance AccelStateData accelState GyroStateData gyros int32_t retval = 0 gyros.x = 0.0f gyros.y = 0.0f gyros.z = 0.0f portTickType startTime = xTaskGetTickCount pseudo_windowed_variance_init &gyro_var 0 VARIANCE_WINDOW_SIZE pseudo_windowed_variance_init &gyro_var 1 VARIANCE_WINDOW_SIZE pseudo_windowed_variance_init &gyro_var 2 VARIANCE_WINDOW_SIZE if init == 0 && initialZeroWhenBoardSteady pseudo_windowed_variance_push_sample &gyro_var 0 gyros.x pseudo_windowed_variance_push_sample &gyro_var 1 gyros.y pseudo_windowed_variance_push_sample &gyro_var 2 gyros.z float const gyrovarx = pseudo_windowed_variance_get &gyro_var 0 float const gyrovary = pseudo_windowed_variance_get &gyro_var 1 float const gyrovarz = pseudo_windowed_variance_get &gyro_var 2 if fabsf gyrovarx + fabsf gyrovary + fabsf gyrovarz > boardSteadyMaxVariance startTime = xTaskGetTickCount if xTaskGetTickCount - startTime < 1000 PIOS_NOTIFY_StartNotification NOTIFY_OK NOTIFY_PRIORITY_REGULAR else if xTaskGetTickCount - startTime < 7000 initialZeroWhenBoardSteady = attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE boardSteadyMaxVariance = attitudeSettings.BoardSteadyMaxVariance
LibrePilot 15064 attitude.c Control c090b8df38 LP-327 - Raise an alarm to prevent arming if gyro is not yet calibrated (implement the same Revo behaviour) if retval != 0 || init == 0
LibrePilot 15073 attitude.c Non-Control ad0cf537f5 LP-327 - fixes from review * @author The LibrePilot Project http: * The OpenPilot Team http:
LibrePilot 16008 attitude.c Non-Control b2f548b2ed LP-173 Add settings initcall type. Add SETTINGS_INITIALISE_ALL to board files. Remove all manual settings objects initializations.
LibrePilot 12071 filterbaro.c Non-Control 741c70cda4 barometric bias filter added #include inc/stateestimation.h #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f static float baroOffset = 0.0f static bool first_run = 1 static int32_t init void static int32_t filter stateEstimation *state void filterBaroInitialize stateFilter *handle handle->init = &init handle->filter = &filter static int32_t init void baroOffset = 0.0f first_run = 1 return 0 static int32_t filter stateEstimation *state if first_run if ISSET state->updated bar_UPDATED first_run = 0 baroOffset = state->bar 0 else if ISSET state->updated pos_UPDATED baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -state->pos 2 - state->bar 0 if ISSET state->updated bar_UPDATED state->bar 0 -= baroOffset return 0
LibrePilot 12097 filterbaro.c Non-Control b56de3b66b filterchain rework #define STACK_REQUIRED 64 struct data float baroOffset bool first_run static int32_t init stateFilter *self static int32_t filter stateFilter *self stateEstimation *state int32_t filterBaroInitialize stateFilter *handle handle->init = &init handle->filter = &filter handle->localdata = pvPortMalloc sizeof struct data return STACK_REQUIRED static int32_t init stateFilter *self struct data *this = struct data * self->localdata this->baroOffset = 0.0f this->first_run = 1 static int32_t filter stateFilter *self stateEstimation *state struct data *this = struct data * self->localdata if this->first_run if ISSET state->updated SENSORUPDATES_baro this->first_run = 0 this->baroOffset = state->baro 0 if ISSET state->updated SENSORUPDATES_pos this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + 1.0f - BARO_OFFSET_LOWPASS_ALPHA * -state->pos 2 - state->baro 0 if ISSET state->updated SENSORUPDATES_baro state->baro 0 -= this->baroOffset
LibrePilot 12118 filterbaro.c Non-Control 843db63cf6 some last issues from review if IS_SET state->updated SENSORUPDATES_baro if IS_SET state->updated SENSORUPDATES_pos if IS_SET state->updated SENSORUPDATES_baro
LibrePilot 12140 filterbaro.c Control 87b1b4873c bugfix in baro alt filter plugin float baroAlt this->baroAlt = state->baro 0 * this->baroAlt + state->pos 2 this->baroAlt = state->baro 0
LibrePilot 12340 filterbaro.c Control 7745f93935 changed default coefficients and exposed them to a settings uavobject as they should be float baroOffset float baroAlt int16_t first_run this->first_run = 100 this->baroOffset = 100.f - this->first_run / 100.f * this->baroOffset + this->first_run / 100.f * state->baro 0 this->baroAlt = this->baroOffset this->first_run-- UNSET_MASK state->updated SENSORUPDATES_baro
LibrePilot 12403 filterbaro.c Control aa560653d4 Make baro altitude offset (relative to GPS alt) filter configurable #include float baroGPSOffsetCorrectionAlpha RevoSettingsData revoSettings RevoSettingsInitialize RevoSettingsGet &revoSettings this->baroGPSOffsetCorrectionAlpha = revoSettings.BaroGPSOffsetCorrectionTau < 0.0001f ? 1.0f : expf - 1.0f / revoSettings.BaroGPSOffsetCorrectionFrequency / revoSettings.BaroGPSOffsetCorrectionTau this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha + 1.0f - this->baroGPSOffsetCorrectionAlpha * this->baroAlt + state->pos 2
LibrePilot 12422 filterbaro.c Non-Control 1bfa66c2b6 Change settings for baro altitude offset correction RevoSettingsBaroGPSOffsetCorrectionAlphaGet &this->baroGPSOffsetCorrectionAlpha
LibrePilot 12587 filterbaro.c Control 66b0ffd682 small fix to baro state filter - more init cycles #define INIT_CYCLES 1000 this->first_run = INIT_CYCLES this->baroOffset = INIT_CYCLES - this->first_run / INIT_CYCLES * this->baroOffset + this->first_run / INIT_CYCLES * state->baro 0
LibrePilot 12588 filterbaro.c Control 84af4b7651 fixes to barofilter #define INIT_CYCLES 100 this->baroOffset = float INIT_CYCLES -this->first_run / float INIT_CYCLES *this->baroOffset + this->first_run / float INIT_CYCLES * state->baro 0 this->baroAlt = 0
LibrePilot 12946 filterbaro.c Control 011c598793 OP-1296 only use GPS in filterchains when explicitly requesteddisallow arming when GPS is needed for initialization until lock is acquired #define STACK_REQUIRED 128 float gpsAlt bool useGPS static int32_t initwithgps stateFilter *self static int32_t initwithoutgps stateFilter *self static int32_t maininit stateFilter *self handle->init = &initwithgps int32_t filterBaroiInitialize stateFilter *handle handle->init = &initwithoutgps handle->filter = &filter handle->localdata = pvPortMalloc sizeof struct data return STACK_REQUIRED static int32_t initwithgps stateFilter *self struct data *this = struct data * self->localdata this->useGPS = 1 return maininit self static int32_t initwithoutgps stateFilter *self struct data *this = struct data * self->localdata this->useGPS = 0 return maininit self static int32_t maininit stateFilter *self this->gpsAlt = 0.0f if this->useGPS && IS_SET state->updated SENSORUPDATES_pos if this->first_run == INIT_CYCLES this->gpsAlt = state->pos 2 this->first_run-- if this->first_run < INIT_CYCLES || !this->useGPS this->baroOffset = float INIT_CYCLES -this->first_run / float INIT_CYCLES * this->baroOffset + this->first_run / float INIT_CYCLES * state->baro 0 + this->gpsAlt this->baroAlt = state->baro 0 this->first_run-- return 2 if this->useGPS && IS_SET state->updated SENSORUPDATES_pos return 0
LibrePilot 13048 filterbaro.c Control 6d3ade4947 OP-1308 Set the same logic for alarms static filterResult filter stateFilter *self stateEstimation *state static filterResult filter stateFilter *self stateEstimation *state return FILTERRESULT_CRITICAL return FILTERRESULT_OK
LibrePilot 13278 filterbaro.c Control 6226aec2a7 OP-943 uses pios_mem API for heap allocation handle->localdata = pios_malloc sizeof struct data handle->localdata = pios_malloc sizeof struct data
LibrePilot 16026 filterbaro.c Non-Control b2f548b2ed LP-173 Add settings initcall type. Add SETTINGS_INITIALISE_ALL to board files. Remove all manual settings objects initializations.
LibrePilot 16077 filterbaro.c Control 8cc85923d7 LP-560 Add better filtering before set the baroOffset value * @author The LibrePilot Project http: * The OpenPilot Team http: #define STACK_REQUIRED 128 #define INIT_CYCLES 500 #define BARO_OFFSET_ALPHA 0.02f if this->first_run > INIT_CYCLES - 2 this->baroOffset = state->baro 0 + this->gpsAlt this->baroOffset = 1.0f - BARO_OFFSET_ALPHA * this->baroOffset + BARO_OFFSET_ALPHA * state->baro 0 + this->gpsAlt
LibrePilot 6305 pid.c Non-Control 50c7641162 Move the PID methods into a standalone library #include openpilot.h #include pid.h static float bound float val float range float pid_apply struct pid *pid const float err float dT float diff = err - pid->lastErr pid->lastErr = err pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f return err * pid->p + pid->iAccumulator / 1000.0f + diff * pid->d / dT void pid_zero struct pid *pid if !pid return pid->iAccumulator = 0 pid->lastErr = 0 void pid_configure struct pid *pid float p float i float d float iLim if !pid return pid->p = p pid->i = i pid->d = d pid->iLim = iLim static float bound float val float range if val < -range val = -range else if val > range val = range return val
LibrePilot 6437 pid.c Control fa9a616b4c PID: Add the 20 Hz low pass filter to the derivative term float dterm = 0 if pid->d && dT dterm = pid->lastDer + dT / dT + 7.9577e-3f * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm pid->lastDer = 0
LibrePilot 6452 pid.c Control 42bbd52d68 PID: Add a function to set the cutoff for the derivative termAlso contains a term for setting the deriative setpoint in the future #define F_PI float M_PI static float deriv_tau = 7.9577e-3f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer void pid_configure_derivative float cutoff float gamma deriv_tau = 1.0f / 2 * F_PI * cutoff
LibrePilot 6454 pid.c Control e4a167dca1 PID: Add a pid_apply_setpoint which takes in the setpoint and feedback termThis version allows performing setpoint weighting currently on the derivativecomponent. static float deriv_gamma = 1.0 pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f if pid->d && dT dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm float pid_apply_setpoint struct pid *pid const float setpoint const float measured float dT float err = setpoint - measured float diff = deriv_gamma * setpoint - measured - pid->lastErr float dterm = 0 pid->lastErr = err void pid_configure_derivative float cutoff float g deriv_gamma = g
LibrePilot 6456 pid.c Control 595083c982 PID: Fix small bug in setpoint weighting that had really ugly consequences float diff = deriv_gamma * setpoint - measured - pid->lastErr pid->lastErr = deriv_gamma * setpoint - measured
LibrePilot 8943 pid.c Non-Control 66db978244 Rename Libraries->libraries Modules->modules
LibrePilot 9934 pid.c Non-Control 5f3e0c3e1d Math cleanup #include deriv_tau = 1.0f / 2 * M_PI_F * cutoff
LibrePilot 9945 pid.c Non-Control 80c917591e Cherry-pick and hand-merge of Sami s math cleanup commit from branch sambas/diffnext: 5f3e0c3e1da9388ca7572a5cb97d67a002dc0360 #include deriv_tau = 1.0f / 2 * M_PI_F * cutoff
LibrePilot 9966 pid.c Control 7937ae6296 OP-931: Makes flight code compile with -Wfloat-equal and -Wunsuffixed-float-constants enabled.Also fixes warnings (and bugs) in F4 STM32_USB_OTG_Driver code allowing -Werror to be enabled for all flight code.Fixes all other compiler warnings that would otherwise cause the flight code to not compile with -Werror enabled.Along the way this also adds some uses of isnan() to various places rather than questionable tests for x != x andx == x to check for NaNs.+review OPReview static float deriv_gamma = 1.0f if pid->d > 0.0f && dT > 0.0f if pid->d > 0.0f && dT > 0.0f
LibrePilot 10961 pid.c Non-Control 80caceb984 make uncrustify_all;make uncrustify_all; static float deriv_tau = 7.9577e-3f pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float diff = err - pid->lastErr float dterm = 0 pid->lastErr = err if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm float err = setpoint - measured pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float dterm = 0 float diff = deriv_gamma * setpoint - measured - pid->lastErr pid->lastErr = deriv_gamma * setpoint - measured if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm if !pid return pid->iAccumulator = 0 pid->lastErr = 0 pid->lastDer = 0 deriv_tau = 1.0f / 2 * M_PI_F * cutoff deriv_gamma = g if !pid return pid->p = p pid->i = i pid->d = d pid->iLim = iLim if val < -range val = -range else if val > range val = range return val
LibrePilot 11546 pid.c Non-Control daf329d8f5 uncrustification (again) static float deriv_tau = 7.9577e-3f pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float diff = err - pid->lastErr float dterm = 0 pid->lastErr = err if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm float err = setpoint - measured pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float dterm = 0 float diff = deriv_gamma * setpoint - measured - pid->lastErr pid->lastErr = deriv_gamma * setpoint - measured if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm if !pid return pid->iAccumulator = 0 pid->lastErr = 0 pid->lastDer = 0 deriv_tau = 1.0f / 2 * M_PI_F * cutoff deriv_gamma = g if !pid return pid->p = p pid->i = i pid->d = d pid->iLim = iLim if val < -range val = -range else if val > range val = range return val
LibrePilot 11805 pid.c Non-Control 900f643bbd Reformat source code with make uncrustify_all run twice. NO CODE CHANGES static float deriv_tau = 7.9577e-3f pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float diff = err - pid->lastErr float dterm = 0 pid->lastErr = err if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm float err = setpoint - measured pid->iAccumulator += err * pid->i * dT * 1000.0f pid->iAccumulator = bound pid->iAccumulator pid->iLim * 1000.0f float dterm = 0 float diff = deriv_gamma * setpoint - measured - pid->lastErr pid->lastErr = deriv_gamma * setpoint - measured if pid->d > 0.0f && dT > 0.0f dterm = pid->lastDer + dT / dT + deriv_tau * diff * pid->d / dT - pid->lastDer pid->lastDer = dterm return err * pid->p + pid->iAccumulator / 1000.0f + dterm if !pid return pid->iAccumulator = 0 pid->lastErr = 0 pid->lastDer = 0 deriv_tau = 1.0f / 2 * M_PI_F * cutoff deriv_gamma = g if !pid return pid->p = p pid->i = i pid->d = d pid->iLim = iLim if val < -range val = -range else if val > range val = range return val
LibrePilot 12418 pid.c Control 2ecbf4a846 add code to scale stabilization PID coefficients by 1/airspeed^2 if the user wishes to do so * @param in factor A dynamic factor to scale pids by to compensate nonlinearities float pid_apply_setpoint_scaled struct pid *pid const float factor const float setpoint const float measured float dT pid->iAccumulator += err * factor * pid->i * dT * 1000.0f dterm = pid->lastDer + dT / dT + deriv_tau * factor * diff * pid->d / dT - pid->lastDer return err * factor * pid->p + pid->iAccumulator / 1000.0f + dterm
LibrePilot 12476 pid.c Non-Control 0724fca32a refactored pid_apply_scaled back into pid_apply (rename only) float pid_apply_setpoint struct pid *pid const float factor const float setpoint const float measured float dT
LibrePilot 12992 pid.c Control e9d1a2af4b OP-1309 get rid of this horrible bound() function code duplication throughout entire flight code and put it into libraries/math #include pid->iAccumulator = boundf pid->iAccumulator pid->iLim * -1000.0f pid->iLim * 1000.0f pid->iAccumulator = boundf pid->iAccumulator pid->iLim * -1000.0f pid->iLim * 1000.0f
LibrePilot 13547 pid.c Control c0152b7e19 OP-1474 Thrust PID scaling float pid_scale_factor_from_line float x struct point *p0 struct point *p1 const float dY1 = p1->y - p0->y const float dX1 = p1->x - p0->x const float m = dY1 / dX1 const float b = p0->y - m * p0->x float y = m * x + b return 1.0f + y float pid_scale_factor pid_scaler *scaler const int length = sizeof scaler->points / sizeof typeof scaler->points 0 int end_point = length - 1 for int i = 1 i < length i++ if scaler->x < scaler->points i .x end_point = i break return pid_scale_factor_from_line scaler->x &scaler->points end_point - 1 &scaler->points end_point float pid_apply_setpoint_scaled struct pid *pid const float factor const float setpoint const float measured float dT pid_scaler *scaler float p = pid->p float d = pid->d float scale = pid_scale_factor scaler pid->p *= scale pid->d *= scale float value = pid_apply_setpoint pid factor setpoint measured dT pid->p = p pid->d = d return value
LibrePilot 13550 pid.c Control 0e2af9c654 OP-1474 Extract line and curve functions out to mathmisc.h nan check and unit tests float y = y_on_curve scaler->x scaler->points sizeof scaler->points / sizeof scaler->points 0 return 1.0f + IS_REAL y ? y : 0.0f
LibrePilot 13552 pid.c Non-Control 662b29ac05 OP-1474 Include I in the scaling and let the TPS factor piggyback on the speedScaleFactor
LibrePilot 13727 pid.c Control 31c5f1c519 OP-1474 Make it possible to select the combination of P I and D to scale float pid_apply_setpoint struct pid *pid const pid_scaler *scaler const float setpoint const float measured float dT pid->iAccumulator += err * scaler->i * pid->i * dT * 1000.0f dterm = pid->lastDer + dT / dT + deriv_tau * scaler->d * diff * pid->d / dT - pid->lastDer return err * scaler->p * pid->p + pid->iAccumulator / 1000.0f + dterm
LibrePilot 13775 pid.c Non-Control 2586dafa8c OP-1511 added comment to filter code for better readability pid->lastDer = dterm
LibrePilot 14118 pid.c Non-Control 88494e2745 OP-1696 PathFollower rewrite in C++ and Landing Squashed commit of the following:commit 580cd8addafadc3109b90b1fd183130ea8add7f2Author: abeck70 Date: Fri Mar 20 21:32:33 2015 +1100 OP-1696 Cleanup and bug fix 1. Code review cleanup 2. Fixed gps assist braking - the thrust limits in piddown were not being set.commit 31ac4494f6e2bc5859f49d1f76f19fe3a4d32862Author: abeck70 Date: Wed Mar 18 08:31:16 2015 +1100 OP-1696 fix initialisation of down pid control parameterscommit a7bd737459249530bde9546b8247dd45b40dced4Author: abeck70 Date: Tue Mar 17 21:18:38 2015 +1100 OP-1769 Actuator fix from code reviewcommit 968172b8fb46c91b2611be4825e17faf2fa13411Author: abeck70 Date: Tue Mar 17 21:01:11 2015 +1100 OP-1696 fixed ground pathfollower settings xmlcommit 721c07b67a87911ed67d0b62aa658cfac6d3eb97Merge: f556215 be9f4aeAuthor: abeck70 Date: Tue Mar 17 20:52:01 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit f556215ddbd5a29b22c2e6124372d74a0d6b9a0fAuthor: abeck70 Date: Tue Mar 17 20:51:22 2015 +1100 OP-1696 ground pathfollower fixescommit 4584b22d6a2d33d3efb1cd11ac46804a5623a236Author: abeck70 Date: Tue Mar 17 18:11:52 2015 +1100 OP-1696 Ground redesign 1. Move to new PID loop design 2. Prototype steering controlcommit b70c3373646fa758ae30a59765b8a56df7d2fbebAuthor: abeck70 Date: Sun Mar 15 18:42:06 2015 +1100 OP-1696 ground pathfollower added missing call to update velocity statecommit a282bfb14dffcd486466b60be3100d26c11649efAuthor: abeck70 Date: Sun Mar 15 17:22:02 2015 +1100 OP-1696 ground pathfollower improvementscommit f56ea4f9b101415e32e4464e51e087ee350be969Author: abeck70 Date: Sun Mar 15 07:44:01 2015 +1100 OP-1696 remove unused PID controller in ground for verticalcommit 6424aa0a84c3718a5a7b51fab1c9e04587d325b8Author: abeck70 Date: Sat Mar 14 21:40:58 2015 +1100 OP-1696 Initialise vtolemergencyfallback variablescommit 5d6110499bb06fe9b397e09dea5aafe994f03d13Author: abeck70 Date: Sat Mar 14 21:30:21 2015 +1100 OP-1696 vtolflycontroller segv fixed and uncrustifycommit bef23ea3dc2b5aff5b9b8713802586719b8f68cfAuthor: abeck70 Date: Sat Mar 14 20:50:05 2015 +1100 OP-1783 PathPlanner Instead of config alarm if no pathplan simply continue to alarm pathplan but allow normal arming and flight in other modes. If pathplan enabled simply enter position hold.commit 370f1747209cca7c01c071a43224ad83b04bdd55Author: abeck70 Date: Sat Mar 14 20:48:22 2015 +1100 OP-1769 Ground code review comment 1. Remove idx1 < 0 check not needed 2. Should return abs_input in the corner case that curve is invalid .commit 01973c656d783e035ad823c267d3387bf473eebeMerge: be07d12 5c9b6faAuthor: abeck70 Date: Sat Mar 14 12:17:31 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit be07d125f42b5ca1aabce0f5c0fcf592e8cddf98Merge: 44afdc7 ad44b31Author: abeck70 Date: Thu Mar 12 21:55:52 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 44afdc77987049dc58cdf42e3be88b7eae38de02Author: abeck70 Date: Thu Mar 12 19:35:55 2015 +1100 OP-1696 fix startup Fix initialisation and void use of pios_malloc at startupcommit 962f5f75756883cec6726c0347310d60264f6989Author: abeck70 Date: Thu Mar 12 18:48:04 2015 +1100 OP-1696 undo changes to make filecommit 7fa71d5c614d492cb4026312672068f92a4bcd3eMerge: 672732e 5a9cfa6Author: abeck70 Date: Thu Mar 12 18:06:59 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 672732e95d555946300ef5093f687f3fa88bfd3aAuthor: abeck70 Date: Wed Mar 11 12:59:44 2015 +1100 OP-1696 code review commentscommit 73b295d6515f2489b3b3a61459c975b0c37afe3dAuthor: abeck70 Date: Wed Mar 11 12:47:24 2015 +1100 OP-1696 code review commentscommit 524b72f537fe922bf3c4385b418872f13819a5cfMerge: 6beef01 ba5f395Author: abeck70 Date: Tue Mar 10 23:19:06 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 6beef01476d06598652a10de4857e6ae55015417Merge: 100cf33 8c31f96Author: abeck70 Date: Sat Mar 7 16:50:32 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 100cf335d187fe696d3678371b194fc3044a1c7eAuthor: Alex Beck Date: Thu Mar 5 22:32:30 2015 +1100 OP-1696 simposix c++ compilation fixcommit 9ebf846d1ec8fef30ffc1204546fc5dc648ec10bAuthor: Alex Beck Date: Thu Mar 5 21:35:39 2015 +1100 OP-1696 fix simposix c++ compile of pathfollowercommit a86b4b1db46dba569739587ee57b03460381ebabAuthor: abeck70 Date: Thu Mar 5 18:03:51 2015 +1100 OP-1696 resolve linux build issuecommit 1d7695f81293534bc17f69b0fcd9de3876867f6aAuthor: abeck70 Date: Wed Mar 4 18:10:09 2015 +1100 OP-1696 Return to Base & Land implemented Enabled via FlightModeSettings ReturnToBaseNextCommand = LANDcommit 4e5ca807b18a40ea60c05c1b46f738462595a6b2Author: abeck70 Date: Wed Mar 4 16:38:06 2015 +1100 OP-1696 reduce memory footprint by only allocating controllers required for a particular frame typecommit 78e8f559cd25ffaeac68b563e25d1ad357205fd6Author: abeck70 Date: Wed Mar 4 13:51:05 2015 +1100 OP-1696 move fsm initialisation to controller implementationscommit 848c78a84e5c7e11b2da953c9d9549b11fc5b1d8Author: abeck70 Date: Wed Mar 4 13:27:11 2015 +1100 OP-1696 reduce memory through re-use of pointers to data objects in base class.commit bc8794601142055942910ba9f938c03c8a3ca547Merge: 24bc3de 843325dAuthor: abeck70 Date: Tue Mar 3 22:06:01 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 24bc3de6155a74850b17ca7706f6b592923e027cAuthor: abeck70 Date: Tue Mar 3 17:56:12 2015 +1100 OP-1696 update gcs path planner options to align to new modescommit dc659891882ea0624a6436550425c54b347f4101Author: abeck70 Date: Tue Mar 3 13:51:55 2015 +1100 OP-1696 pathfollower class renaming and uncrustifycommit af19d114b8e0ff23159bdaca37e5f5bf1c1f8bebMerge: b07c1fd 04f4cd9Author: abeck70 Date: Mon Mar 2 21:47:12 2015 +1100 Merge branch rel-15.02 of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit b07c1fdd0544a555678bc7a8510cd92861f17e46Author: abeck70 Date: Mon Mar 2 21:27:27 2015 +1100 OP-1696 remainder of changes to integrate drive and fly objectivescommit c824c53cf30889a468ecb25ae3d705fa011b23baMerge: b89751c 1102243Author: abeck70 Date: Mon Mar 2 17:06:13 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit b89751caa5d6dc06e1e20391aa4e12b5484056d4Author: abeck70 Date: Mon Mar 2 17:04:27 2015 +1100 OP-1696 merge fly and drive pathdesired/action objectivescommit c463132cb7da5cdf9fc5395ae0650b081bc9fb55Author: abeck70 Date: Sun Mar 1 10:25:29 2015 +1100 OP-1696 merge ground pathfollower and refactored into C++ commit 6ed504b32f9e502e4f02d50b9e85ba91591417ba Merge: bcf245e deafed6 Author: abeck70 Date: Fri Feb 27 18:12:32 2015 +1100 Merge branch next into corvuscorax/OP-1642_tank-support Conflicts: flight/modules/PathFollower/pathfollower.c shared/uavobjectdefinition/vtolpathfollowersettings.xml commit bcf245e08c3348f2e6e5eddceee6d6cc268bdc23 Author: Corvus Corax Date: Sat Dec 6 19:45:29 2014 +0100 OP-1642 added new uavobject to discovery target too commit 6b9e8a7d7dad0739b4ffd2ce7f2f78ed4f597077 Author: Corvus Corax Date: Sat Dec 6 17:58:18 2014 +0100 OP-1642 added very simple path control for ground vehicles commit e8d8440e049c06158e7fc542627fac6420abaf65 Author: Corvus Corax Date: Sat Dec 6 17:17:09 2014 +0100 OP-1642 added new settings uavobject for tank path followercommit 71dd726eb5b52614b417236c22e8a5cc3a487fafAuthor: abeck70 Date: Fri Feb 27 18:05:10 2015 +1100 OP-1696 align pathaction to pathdesiredcommit 8a88af83be457aafbb63e4f94fad3b1cd54264caAuthor: abeck70 Date: Thu Feb 26 08:06:34 2015 +1100 OP-1696 fixed wing control refactored and compilingcommit fd9850b5e5680f5b422dbc468a94c5ec4b6dd518Author: abeck70 Date: Wed Feb 25 17:47:19 2015 +1100 OP-1696 Removed vtol stuff from pathfollower.cpp after having replaced it with class implcommit 9127d8d6d8c345c5bbf8622ce4e5eb76b03d1356Merge: 4054c73 93fb2d5Author: abeck70 Date: Wed Feb 25 13:09:06 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 4054c73ac98b54188a0a5367c5ca913b7fee6a89Author: abeck70 Date: Tue Feb 24 12:29:28 2015 +1100 OP-1696 fix compile errorscommit b398db3d4de8cd621a059da1f0dbb00868be0fb9Merge: 94c795a 3e15cacAuthor: abeck70 Date: Tue Feb 24 12:06:35 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 94c795af386cf0f1072a88e99a80cb1080bf85e7Merge: d2115de 57e0a8bAuthor: abeck70 Date: Mon Feb 23 17:54:33 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 Conflicts: flight/modules/Sensors/sensors.c make/common-defs.mkcommit d2115dee4e8e9b6a02f7b7700e4715480a56b18fAuthor: abeck70 Date: Mon Feb 23 14:35:02 2015 +1100 OP-1696 preliminary checkin for refactor of fly TODO: 1. Finalise and test fly brake 2. Remove patfhollower vtol fly 3. Rename all objects adding vtol in namecommit 95804b1d591d194cce3596187c74cdb2dd0d192fMerge: 58cebb0 31c1385Author: abeck70 Date: Sat Feb 21 22:10:03 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 58cebb0658deaece1555cfa24e50f1b86fd68c96Author: abeck70 Date: Sat Feb 21 22:09:29 2015 +1100 OP-1696 refactor neutral thrust and add pos hold implementationcommit 2c3cadfef3de9e092bd66bbabc58ac7847aca7e4Author: abeck70 Date: Fri Feb 20 18:32:11 2015 +1100 OP-1696 start of refactor of brakingcommit 991c2124e834f3432f5e34c5c9f4966cb42a7be3Merge: 1ab5798 db7d605Author: abeck70 Date: Thu Feb 19 12:17:54 2015 +1100 Merge branch next of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696commit 1ab579865f04aeaa5899f5065a983de29074e025Author: abeck70 Date: Wed Feb 18 21:29:01 2015 +1100 OP-1696 Rewrote velocity roam into new C++ OO architecture With use of new PID controller for NE.commit 03edb049f06bf60054dce36a5b8b301a397284c4Author: abeck70 Date: Wed Feb 18 12:19:17 2015 +1100 OP-1696 Start of refactoring of VelocityRoam implementation in PathFollower Also TODOs: 1. To support pathplanner remove all references to FlightStatus object and add more fields to PathDesired 2. Refactor Braking to use C++ 3. Test and optimise Velocity Roam 4. Add more config options to Landing. For example disable disarming. And add more setting objects. 5. Consider moving all landing settings to one object.commit 1f5dba821d4535a9a14f333efdeec1af5afcc281Author: abeck70 Date: Mon Feb 16 22:51:31 2015 +1100 OP-1696 Landing tuningcommit 5133956c717fba871dd5700333806e298038a3d0Author: abeck70 Date: Mon Feb 16 21:05:06 2015 +1100 OP-1696 Landing Multiple fixescommit cf22055ecb8eb7304174c730f249a2332ebc599bAuthor: abeck70 Date: Sun Feb 15 20:52:29 2015 +1100 OP-1696 Add PIDStatus debug objectcommit bc13d732601e7c7cb48db17f1a75b33cc96c0c34Author: abeck70 Date: Sat Feb 14 15:53:16 2015 +1100 OP-1696 Landing sanity testedcommit 6e913076c1dfcb8b90153e9eca8acf6c9e51f293Author: abeck70 Date: Sat Feb 14 11:16:06 2015 +1100 OP-1696 completed refactoring 1. Completed refactoring of NE and Thrust control 2. Fixed velocity roam TODO Tuning and testingcommit 63b62fbd0ac105029e54163b98c3beebc2d73c3aAuthor: abeck70 Date: Fri Feb 13 08:54:23 2015 +1100 OP-1696 New PID Controller and refactoring of PID implementation for thrust managementcommit 7474331529477d0badd2ef0d609c84c556be9cecAuthor: abeck70 Date: Tue Feb 10 16:39:44 2015 +1100 OP-1696 Landing Refactor Thrust PID Loop for landingcommit 904ab2a9241e40c19da6a1508b4ce5243c75d7a3Author: abeck70 Date: Mon Feb 9 22:13:39 2015 +1100 OP-1696 Uncrustifycommit b3d30e298d8a0343782a29bc91be6677105b325eAuthor: abeck70 Date: Mon Feb 9 22:05:05 2015 +1100 OP-1696 Create new OO design Added PathFollowerControl which has an Landing implementation. The landing implementation runs its own set of PIDs. And interfaces to the FSM for Landing.commit 1ffd8d67cf7310c1c18c90f3ba18b08221d185a3Author: abeck70 Date: Sat Feb 7 18:04:42 2015 +1100 OP-1696 Upgrade arm toolscommit 3df01f97021d13694027b92ba0b13cc9fe70d437Author: abeck70 Date: Fri Feb 6 21:52:38 2015 +1100 OP-1696 Landing uncrustify and start of refactoring landing to make more use of FSMLandcommit 7a753b65fb69d52c89875b9a182ef5210ee8ef2dAuthor: abeck70 Date: Fri Feb 6 15:08:29 2015 +1100 OP-1696 Landing: fix compilation of all exescommit 692459874acf2b7ae07aa3ab88a2c78a09f26ee6Author: abeck70 Date: Fri Feb 6 14:45:06 2015 +1100 OP-1696 Landing startup fixescommit 9851f29b9238a969efd0110f01a4fad6bdbc5a5dAuthor: abeck70 Date: Fri Feb 6 10:21:52 2015 +1100 OP-1696 Landing pathfollower CPP refactoringcommit 9b3a54d41f14f07cd0d78426026d12ae9ac4e5b5Author: abeck70 Date: Wed Feb 4 13:02:44 2015 +1100 OP-1696 Moved to FSMLand class and fixed CPP problems 1. Additional methods compile options and need to have main as a cpp to solve various link issues when using static constructors but to avoid adding unnecessary libs.commit 7ebe739bfdb4e0f598f4408c1488f01bc033c044Author: abeck70 Date: Tue Feb 3 22:17:08 2015 +1100 OP-1696 Initial Landing prototype Proof of concept. To be refactored into C++commit 11d6754b9766efb76cc1cd57e9894badaf33ef2dAuthor: abeck70 Date: Tue Feb 3 22:13:10 2015 +1100 OP-1696 CPP Enablement revo c++ Enable optional revo C++ support: 1. Set USE_CXX to enable compliation and linkage of C++ source code 2. Disables rtti and exceptions 3. operator new and delete call pios malloc/free 4. Static constructor invocation supported void pid2_configure struct pid2 *pid float kp float ki float kd float Tf float kt float dT float beta float u0 float va float vb pid->reconfigure = true pid->u0 = u0 pid->va = va pid->vb = vb pid->kp = kp pid->beta = beta pid->bi = ki * dT pid->br = kt * dT / vb pid->ad = Tf / Tf + dT pid->bd = kd / Tf + dT void pid2_transfer struct pid2 *pid float u0 pid->reconfigure = true pid->u0 = u0 float pid2_apply struct pid2 *pid const float r const float y const float ulow const float uhigh if pid->reconfigure pid->reconfigure = false pid->yold = y pid->D = 0.0f pid->I = pid->u0 - pid->va / pid->vb - pid->kp * pid->beta * r - y pid->P = pid->kp * pid->beta * r - y pid->D = pid->ad * pid->D - pid->bd * y - pid->yold float v = pid->va + pid->vb * pid->P + pid->I + pid->D float u = boundf v ulow uhigh pid->I = pid->I + pid->bi * r - y + pid->br * u - v pid->yold = y return u
LibrePilot 14865 pid.c Control a513b2c675 [LP-276] Support D Term calculation on measurement only float pid_apply_setpoint struct pid *pid const pid_scaler *scaler const float setpoint const float measured float dT bool meas_based_d_term float diff float derr = -measured if !meas_based_d_term derr += deriv_gamma * setpoint diff = derr - pid->lastErr pid->lastErr = derr
LibrePilot 15393 pid.c Non-Control c723a06e93 LP-289 added comments and fixed one typo * @author The LibrePilot Project http: * The OpenPilot Team http: * @param in Tf filtering time = kd/kp /N N is in the range of 2 to 20
LibrePilot 15394 pid.c Non-Control ebc4b16d9a LP-289 Clean up comments and uncrustify * The OpenPilot Team http:
LibrePilot 15395 pid.c Control fb5f9034ea LP-289 prevent initialization with non-zero pid->I accumulator (dangerous if kI is low) pid->I = 0.0f
LibrePilot 15396 pid.c Non-Control dcb3d760db LP-289 Updated copyright notices * @author The LibrePilot Project http:
LibrePilot 15397 pid.c Non-Control c056e73b93 LP-289 Removed orphaned doxygen addtogroup directives
LibrePilot 16095 pid.c Control 691c6c1b57 LP-596 Fix AltitudeHold transition pid->I = pid->u0 - pid->va / pid->vb if pid->bi > 5e-7f pid->I -= pid->kp * pid->beta * r - y

Results


  Machine learning models

Our machine learning models are created using WEKA tool. To reproduce the results and use the model, make sure to download and install the tool in your computer.

Machine learning models for Research Question 1:

Machine learning models for Research Question 3. There are two steps involved, first load the below model and re-evaluate the model using the LibrePilot test set:



  Java API

You can use the chosen classifier in your own JAVA code as follows. Do not forget imports: 
					
import weka.core.Instances;
import weka.core.SerializationHelper;
import weka.core.converters.ConverterUtils.DataSource;
import weka.classifiers.Evaluation;
import weka.classifiers.meta.AutoWEKAClassifier;


In the below code, replace "Relative path to file" to the actual file location of the above downloaded files.

  GUI

For WEKA GUI, open "Explorer" tab. For Research Question 1, under the "classify" tab, right-click the empty "Result list" section and select "Load model" option. Locate the above downloaded model file.

For Research Question 3, first load one of the above model - either comment or code. Then select "Supplied test set" under the "Test options". Click "set" to locate the LibrePilot dataset for comment or code based cross-project testing. Lastly, right-click the model under "Result list" section and select "Re-evaluate model on current test set".

Resources


  Download PDF

This research work is accepted in the journal ACM Transactions on Cyber-Physical Systems on June 2024. <<< Download link will be provided shortly. >>>

  NSF awards

This work was partially supported by the following awards: National Science Foundation CNS-2047971, CCF-1755890, CCF-1618132, and CCF-2139845, and OAC-2221648, and United States Department of Agriculture (USDA) #2023-67021-38977. Any opinions, findings, conclusions, or recommendations expressed in this material are those of the authors and do not necessarily reflect the views of these agencies.

  Cite

BibTeX
@article{balaji2024carvingoutcontrol, title={Carving out Control Code: Automated Identification of Control Software in Autopilot Systems}, author={Balasubramaniam, Balaji and Ahmed, Iftekhar and Bagheri, Hamid and Bradley, Justin}, journal={ACM Transactions on Cyber-Physical Systems}, volume={}, number={}, pages={--}, year={2024}, publisher={ACM New York, NY} }
MLA
Balasubramaniam, Balaji, et al. "Carving out Control Code: Automated Identification of Control Software in Autopilot Systems." ACM Transactions on Cyber-Physical Systems -.- (2024): --.
APA
Balasubramaniam, B., Ahmed, I., Bagheri, H., & Bradley, J. (2024). Carving out Control Code: Automated Identification of Control Software in Autopilot Systems. ACM Transactions on Cyber-Physical Systems, -(-), --.
Chicago
Balasubramaniam, Balaji and Ahmed, Iftekhar and Bagheri, Hamid and Bradley, Justin. "Carving out Control Code: Automated Identification of Control Software in Autopilot Systems." ACM Transactions on Cyber-Physical Systems -, no. - (2024): --.
Harvard
Balasubramaniam, B., Ahmed, I., Bagheri, H., & Bradley, J., 2024. Carving out Control Code: Automated Identification of Control Software in Autopilot Systems. ACM Transactions on Cyber-Physical Systems, 9(1), pp.--.

Contact us

We would love to hear from you!

Email the authors

223 Schorr Center
Computer Science and Engineering Department
University of Nebraska – Lincoln
Lincoln, NE 68588.