Deepanshu/maximum arming angle#198
Conversation
lib/Espfc/src/Control/Fusion.cpp
Outdated
| // | ||
| // Convert trim from 0.1 degrees to radians and apply. | ||
| euler.y += _model.config.accel.trim[0] * 0.1f * (PI / 180.0f); // pitch trim | ||
| euler.x += _model.config.accel.trim[1] * 0.1f * (PI / 180.0f); // roll trim |
There was a problem hiding this comment.
This solution is not correct. It'll cause that euler angles are out of allowed range for AHRS algorithms. It may result in very bad behaviour in some specific attitude. My recommendation is to build rotation matrix and apply rotation to accel readings somehere in AccelSensor class.
There was a problem hiding this comment.
I see, sure I'll try to build rotation matrix then. thanks
lib/Espfc/src/ModelConfig.h
Outdated
| }; | ||
|
|
||
| struct FusionConfig | ||
| namespace Espfc |
There was a problem hiding this comment.
such reformatting kills whole change history in git. Please leave only changed lines.
There was a problem hiding this comment.
My bad, my formatter just formatted it automatically, didn't noticed. Will revert these formatting changes
lib/Espfc/src/Connect/Cli.cpp
Outdated
| Param(PSTR("failsafe_delay"), &c.failsafe.delay), | ||
| Param(PSTR("failsafe_kill_switch"), &c.failsafe.killSwitch), | ||
|
|
||
| Param(PSTR("arming_auto_disarm_delay"), &c.arming.autoDisarmDelay), |
There was a problem hiding this comment.
do not add fields that are not handled in any way.
There was a problem hiding this comment.
Yeah sorry, my bad. Reverted
lib/Espfc/src/Control/Actuator.cpp
Outdated
| { | ||
| const float maxTiltRad = Utils::toRad(_model.config.arming.smallAngle); | ||
| const float currentTilt = std::max(std::abs(_model.state.attitude.euler[AXIS_PITCH]), | ||
| std::abs(_model.state.attitude.euler[AXIS_ROLL])); |
There was a problem hiding this comment.
technically correct formula should be acos(cos(roll)*cos(pitch)) here
There was a problem hiding this comment.
I have used the approach that you suggested but the tilt calculation using trigonometric functions (acosf, cosf) is computationally more expensive than the previous max(abs(pitch), abs(roll)) approach. This code runs in the arming check path which may execute frequently.
Is the increased accuracy justifies the performance cost ?
There was a problem hiding this comment.
You are right, I'm still wondering if your initial solution will be sufficient. Although the actual solution is more computationally complex, this fragment is only executed 50 times per second. The total impact can be seen in the CLI by entering the "stats" command. The result will then be in the "rx_a:" line. Of course, we can still ask the AI to propose an intermediate solution. Give me some more time to think about it.
This commit introduces a mechanism to apply pitch and roll trim adjustments to the accelerometer readings when the calibration state is idle. The adjustments are based on configuration values and are applied using a rotation matrix to ensure accurate sensor data representation.
lib/Espfc/src/Sensor/AccelSensor.cpp
Outdated
| if(trimPitch != 0.f || trimRoll != 0.f) | ||
| { | ||
| RotationMatrixFloat trimRotation; | ||
| trimRotation.init(VectorFloat(trimRoll, trimPitch, 0.f)); |
There was a problem hiding this comment.
On the other hand init() is pretty expensive function, and executed at about 500Hz. it would be good if we can move it somewhere to the begin() section. If we move trimRotation to the ModelState we would be able to recalculate it only if trims changes via msp.
There was a problem hiding this comment.
Sure, I'll work on this.
…and structure - Updated ModelState.h to enhance code formatting and organization, including consistent indentation and spacing. - Moved enums and structs within the Espfc namespace to improve encapsulation. - Refactored AccelSensor class methods for clarity, including the addition of updateTrimRotation method. - Improved initialization and calibration logic in AccelSensor to streamline the setup process. - Enhanced debug logging for better traceability during sensor initialization and updates.
2 participants
Changes in this pr
This pull request introduces new configuration options related to arming logic and accelerometer trim, along with improvements to attitude calculation and arming safety checks. The changes enhance configurability and safety by allowing fine-tuning of arming behavior and ensuring the aircraft cannot be armed when tilted beyond a user-defined angle. Additionally, accelerometer trim values are now applied to attitude calculations, improving accuracy.
Made minimum arming angle work.
(previously it was default to 180 deg which can result in accidents because it can arm in any angle)
Fixed Accelerometer trim to work from UI.
(This is very useful as fix drone drifts)