diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 42449567ac..b009bfed04 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -530,11 +530,27 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } else { for (int i = 0; i < (int)num_outputs; i++) { - if (outputs[i] == DSHOT_DISARM_VALUE) { + + uint16_t output = outputs[i]; + + // DShot 3D splits the throttle ranges in two. + // This is in terms of DShot values, code below is in terms of actuator_output + // Direction 1) 48 is the slowest, 1047 is the fastest. + // Direction 2) 1049 is the slowest, 2047 is the fastest. + if (_param_dshot_3d_enable.get()) { + if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) { + output = DSHOT_DISARM_VALUE; + + } else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used + output = 999 - output; + } + } + + if (output == DSHOT_DISARM_VALUE) { up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); } else { - up_dshot_motor_data_set(i, math::min(outputs[i], static_cast(DSHOT_MAX_THROTTLE)), + up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)), i == requested_telemetry_index); } } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 31f009a21c..51316fb0ba 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -231,6 +231,9 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_dshot_config, (ParamFloat) _param_dshot_min, + (ParamBool) _param_dshot_3d_enable, + (ParamInt) _param_dshot_3d_dead_h, + (ParamInt) _param_dshot_3d_dead_l, (ParamInt) _param_mot_pole_count ) }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index a276ceda75..850aaf69df 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -14,7 +14,7 @@ parameters: long: | This enables/disables DShot. The different modes define different speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes. - + Note: this enables DShot on the FMU outputs. For boards with an IO it is the AUX outputs. type: enum @@ -40,6 +40,37 @@ parameters: decimal: 2 increment: 0.01 default: 0.055 + DSHOT_3D_ENABLE: + description: + short: Allows for 3d mode when using DShot and suitable mixer + long: | + WARNING: ESC must be configured for 3D mode, and DSHOT_MIN set to 0. + This splits the throttle ranges in two. + Direction 1) 48 is the slowest, 1047 is the fastest. + Direction 2) 1049 is the slowest, 2047 is the fastest. + When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. + type: boolean + default: 0 + DSHOT_3D_DEAD_H: + description: + short: DSHOT 3D deadband high + long: | + When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin. + This value is with respect to the mixer_module range (0-1999), not the DSHOT values. + type: int32 + min: 1000 + max: 1999 + default: 1000 + DSHOT_3D_DEAD_L: + description: + short: DSHOT 3D deadband low + long: | + When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin. + This value is with respect to the mixer_module range (0-1999), not the DSHOT values. + type: int32 + min: 0 + max: 1000 + default: 1000 MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group description: short: Number of magnetic poles of the motors @@ -51,4 +82,3 @@ parameters: Typical motors for 5 inch props have 14 poles. type: int32 default: 14 -