Skip to content

Deepanshu/maximum arming angle #198

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
scientistPargain wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Deepanshu/maximum arming angle #198

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
00e9e59
Add arming configuration parameters and update processing logic
scientistPargain Jan 16, 2026
e435227
Refactor code structure for improved readability and maintainability
scientistPargain Jan 16, 2026
e031996
Add trim adjustments for accelerometer calibration
scientistPargain Feb 1, 2026
ec794df
Refactor ModelState and AccelSensor classes for improved readability …
scientistPargain Feb 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions lib/Espfc/src/Connect/Cli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -506,6 +506,10 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
Param(PSTR("failsafe_delay"), &c.failsafe.delay),
Param(PSTR("failsafe_kill_switch"), &c.failsafe.killSwitch),

Param(PSTR("arming_auto_disarm_delay"), &c.arming.autoDisarmDelay),
Copy link
Copy Markdown
Owner

@rtlopez rtlopez Jan 28, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do not add fields that are not handled in any way.

Copy link
Copy Markdown
Author

@scientistPargain scientistPargain Feb 1, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah sorry, my bad. Reverted

Param(PSTR("arming_disarm_kill_switch"), &c.arming.disarmKillSwitch),
Param(PSTR("arming_small_angle"), &c.arming.smallAngle),

Param(PSTR("vtx_power"), &c.vtx.power),
Param(PSTR("vtx_channel"), &c.vtx.channel),
Param(PSTR("vtx_band"), &c.vtx.band),
Expand Down
21 changes: 16 additions & 5 deletions lib/Espfc/src/Connect/MspProcessor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -504,8 +504,13 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
break;

case MSP_ACC_TRIM:
r.writeU16(0); // pitch
r.writeU16(0); // roll
r.writeU16(_model.config.accel.trim[0]); // pitch
r.writeU16(_model.config.accel.trim[1]); // roll
break;

case MSP_SET_ACC_TRIM:
_model.config.accel.trim[0] = m.readU16(); // pitch
_model.config.accel.trim[1] = m.readU16(); // roll
break;

case MSP_MIXER_CONFIG:
Expand Down Expand Up @@ -768,9 +773,15 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
break;

case MSP_ARMING_CONFIG:
r.writeU8(5); // auto_disarm delay
r.writeU8(0); // disarm kill switch
r.writeU8(180); // small angle
r.writeU8(_model.config.arming.autoDisarmDelay); // auto_disarm delay
r.writeU8(_model.config.arming.disarmKillSwitch); // disarm kill switch
r.writeU8(_model.config.arming.smallAngle); // small angle
break;

case MSP_SET_ARMING_CONFIG:
_model.config.arming.autoDisarmDelay = m.readU8(); // auto_disarm delay
_model.config.arming.disarmKillSwitch = m.readU8(); // disarm kill switch
_model.config.arming.smallAngle = m.readU8(); // small angle
break;

case MSP_RC_DEADBAND:
Expand Down
15 changes: 15 additions & 0 deletions lib/Espfc/src/Control/Actuator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "Control/Actuator.h"
#include "Utils/Math.hpp"
#include <algorithm>
#include <cmath>

namespace Espfc::Control {

Expand Down Expand Up @@ -96,6 +98,19 @@ void Actuator::updateArmingDisabled()
_model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive());
_model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED);
_model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE);

// Check small angle - prevent arming if tilted beyond configured angle
if(_model.config.arming.smallAngle < 180 && _model.accelActive())
{
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]));
Copy link
Copy Markdown
Owner

@rtlopez rtlopez Jan 28, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

technically correct formula should be acos(cos(roll)*cos(pitch)) here

Copy link
Copy Markdown
Author

@scientistPargain scientistPargain Feb 1, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 ?

Copy link
Copy Markdown
Owner

@rtlopez rtlopez Feb 3, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Copy Markdown
Author

@scientistPargain scientistPargain Feb 8, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure

_model.setArmingDisabled(ARMING_DISABLED_ANGLE, currentTilt > maxTiltRad);
}
else
{
_model.setArmingDisabled(ARMING_DISABLED_ANGLE, false);
}
if(_model.isFeatureActive(FEATURE_GPS))
{
_model.setArmingDisabled(ARMING_DISABLED_GPS, !_model.state.gps.present || _model.state.gps.numSats < _model.config.gps.minSats);
Expand Down
30 changes: 24 additions & 6 deletions lib/Espfc/src/Control/Fusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ void Fusion::experimentalFusion()

void Fusion::simpleFusion()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose = getAccelEulerWithTrim();
_model.state.attitude.euler.x = _model.state.pose.x;
_model.state.attitude.euler.y = _model.state.pose.y;
_model.state.attitude.euler.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.z;
Expand All @@ -114,7 +114,7 @@ void Fusion::simpleFusion()

void Fusion::kalmanFusion()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose = getAccelEulerWithTrim();
_model.state.pose.z = _model.state.attitude.euler.z;
const float dt = _model.state.gyro.timer.intervalf;
for(size_t i = 0; i < 3; i++)
Expand All @@ -128,7 +128,7 @@ void Fusion::kalmanFusion()

void Fusion::complementaryFusion()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose = getAccelEulerWithTrim();
_model.state.pose.z = _model.state.attitude.euler.z;
const float dt = _model.state.gyro.timer.intervalf;
const float alpha = 0.002f;
Expand All @@ -147,7 +147,7 @@ void Fusion::complementaryFusionOld()
{
const float alpha = 0.01f;
const float dt = _model.state.gyro.timer.intervalf;
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose = getAccelEulerWithTrim();
_model.state.pose.z = _model.state.attitude.euler.z;
_model.state.attitude.euler = (_model.state.attitude.euler + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha;
_model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion();
Expand Down Expand Up @@ -213,9 +213,27 @@ void Fusion::rtqfFusion()
_model.state.attitude.euler.eulerFromQuaternion(fusionQPose);
}

VectorFloat Fusion::getAccelEulerWithTrim() const
{
VectorFloat euler = _model.state.accel.adc.accelToEuler();
// Euler ordering and aircraft axes mapping:
// euler.x -> roll (rotation about body X)
// euler.y -> pitch (rotation about body Y)
// euler.z -> yaw (rotation about body Z)
//
// Accelerometer trim values are stored in 0.1 degree units:
// trim[0] = pitch trim, applied to euler.y
// trim[1] = roll trim, applied to euler.x
//
// 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
Copy link
Copy Markdown
Owner

@rtlopez rtlopez Jan 28, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Copy Markdown
Author

@scientistPargain scientistPargain Jan 31, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see, sure I'll try to build rotation matrix then. thanks

return euler;
}

void Fusion::updatePoseFromAccelMag()
{
_model.state.pose = _model.state.accel.adc.accelToEuler();
_model.state.pose = getAccelEulerWithTrim();
//_model.state.accelPose = _model.state.pose;

if(_model.magActive())
Expand Down Expand Up @@ -274,7 +292,7 @@ void Fusion::lerpFusion()
{
float correctionAlpha = 0.001f; // 0 - 1 => gyro - accel

_model.state.accelPose = _model.state.accel.adc.accelToEuler();
_model.state.accelPose = getAccelEulerWithTrim();
_model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion();

if(_model.magActive())
Expand Down
1 change: 1 addition & 0 deletions lib/Espfc/src/Control/Fusion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ class Fusion
void mahonyFusion();

private:
VectorFloat getAccelEulerWithTrim() const;
Model& _model;
bool _first;
Madgwick _madgwick;
Expand Down
Loading