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 3 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
2 changes: 2 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,8 @@ 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_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
17 changes: 14 additions & 3 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 @@ -770,7 +775,13 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
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.smallAngle); // small angle
break;

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

case MSP_RC_DEADBAND:
Expand Down
14 changes: 14 additions & 0 deletions lib/Espfc/src/Control/Actuator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,20 @@ 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 roll = _model.state.attitude.euler[AXIS_ROLL];
const float pitch = _model.state.attitude.euler[AXIS_PITCH];
const float currentTilt = acosf(cosf(roll) * cosf(pitch));
_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
7 changes: 7 additions & 0 deletions lib/Espfc/src/ModelConfig.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -591,6 +591,7 @@ struct AccelConfig
int8_t bus = BUS_AUTO;
int8_t dev = GYRO_AUTO;
int16_t bias[3] = { 0, 0, 0 };
int16_t trim[2] = { 0, 0 };
FilterConfig filter{FILTER_BIQUAD, 15};
};

Expand Down Expand Up @@ -678,6 +679,11 @@ struct LedConfig
int8_t type = 0;
};

struct ArmingConfig
{
uint8_t smallAngle = 25;
};

// persistent data
class ModelConfig
{
Expand All @@ -694,6 +700,7 @@ class ModelConfig
IBatConfig ibat;
VtxConfig vtx;
GpsConfig gps;
ArmingConfig arming;

ActuatorCondition conditions[ACTUATOR_CONDITIONS];
ScalerConfig scaler[SCALER_COUNT];
Expand Down
12 changes: 12 additions & 0 deletions lib/Espfc/src/Sensor/AccelSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,18 @@ int FAST_CODE_ATTR AccelSensor::filter()

calibrate();

if(_model.state.accel.calibrationState == CALIBRATION_IDLE)
{
const float trimPitch = Utils::toRad(_model.config.accel.trim[0] * 0.1f);
const float trimRoll = Utils::toRad(_model.config.accel.trim[1] * 0.1f);
if(trimPitch != 0.f || trimRoll != 0.f)
{
RotationMatrixFloat trimRotation;
trimRotation.init(VectorFloat(trimRoll, trimPitch, 0.f));
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.

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.

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, I'll work on this.

_model.state.accel.adc = trimRotation.apply(_model.state.accel.adc);
}
}

return 1;
}

Expand Down