diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 393dc2259d..f67661d34c 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -618,7 +618,7 @@ bool DShot::process_bdshot_telemetry() } else { // Use previous value (esc_status is indexed by motor_index) - esc.erpm = _esc_status.esc[motor_index].esc_rpm * (_param_mot_pole_count.get() / 2); + esc.erpm = _esc_status.esc[motor_index].esc_rpm * (get_pole_count(motor_index) / 2); } // Extended DShot Telemetry @@ -698,7 +698,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source) // Only use SerialTelemetry eRPM when BDShot is disabled if (!is_bdshot) { _esc_status.esc[motor_index].timestamp = esc.timestamp; - _esc_status.esc[motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + _esc_status.esc[motor_index].esc_rpm = esc.erpm / (get_pole_count(motor_index) / 2); } _esc_status.esc[motor_index].esc_voltage = esc.voltage; @@ -707,7 +707,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source) } else if (source == TelemetrySource::BDShot) { _esc_status.esc[motor_index].timestamp = esc.timestamp; - _esc_status.esc[motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + _esc_status.esc[motor_index].esc_rpm = esc.erpm / (get_pole_count(motor_index) / 2); // Only use BDShot Volt/Curr/Temp when Serial Telemetry is disabled if (!_serial_telemetry_enabled) { @@ -874,6 +874,19 @@ void DShot::handle_esc_request_eeprom(const vehicle_command_s &command) _command_ack_pub.publish(command_ack); } +int DShot::get_pole_count(int motor_index) +{ + int32_t pole_count = 14; + + int num_motors = (int)OutputFunction::MotorMax - ((int)OutputFunction::Motor1 - 1); + + if (motor_index >= 0 && motor_index < num_motors) { + param_get(_param_pole_count_handles[motor_index], &pole_count); + } + + return pole_count; +} + void DShot::update_params() { parameter_update_s pupdate; @@ -893,6 +906,13 @@ void DShot::update_params() _mixing_output.minValue(i) = DSHOT_MIN_THROTTLE; } } + + // Update per-motor pole count param handles + for (int i = 0; i < 12; i++) { + char name[20]; + snprintf(name, sizeof(name), "DSHOT_MOT_POL%d", i + 1); + _param_pole_count_handles[i] = param_find(name); + } } void DShot::mixerChanged() @@ -1052,9 +1072,17 @@ int DShot::print_status() } PX4_INFO(" ESC Type: %s (%ld)", esc_type_str, _param_dshot_esc_type.get()); - PX4_INFO(" Motor Poles: %ld", _param_mot_pole_count.get()); PX4_INFO(" 3D Mode: %s", _param_dshot_3d_enable.get() ? "Enabled" : "Disabled"); + // Per-motor pole counts + PX4_INFO(" Motor Poles:"); + + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + if (_motor_mask & (1 << i)) { + PX4_INFO(" Motor %d: %d poles", i + 1, get_pole_count(i)); + } + } + // Telemetry Status if (_bdshot_output_mask || _serial_telemetry_enabled) { print_spacer(); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 220ed0b524..46862f4d24 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -223,6 +223,10 @@ private: uint16_t _programming_address{}; uint16_t _programming_value{}; + // Per-motor pole count + param_t _param_pole_count_handles[12] {}; + int get_pole_count(int motor_index); + // Parameters DEFINE_PARAMETERS( (ParamInt) _param_dshot_esc_type, @@ -230,7 +234,6 @@ private: (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count, (ParamBool) _param_dshot_bidir_edt ) }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index ea00d4a353..3dda5f4dcf 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -72,14 +72,18 @@ parameters: min: 0 max: 1000 default: 1000 - MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group + DSHOT_MOT_POL${i}: description: - short: Number of magnetic poles of the motors + short: Number of magnetic poles of motor ${i} long: | - Specify the number of magnetic poles of the motors. - It is required to compute the RPM value from the eRPM returned with the ESC telemetry. - - Either get the number from the motor spec sheet or count the magnets on the bell of the motor (not the stator magnets). + Number of magnetic poles for motor ${i}. + Required to compute RPM from the eRPM returned by ESC telemetry. + Either get the number from the motor spec sheet or count the magnets + on the bell of the motor (not the stator magnets). Typical motors for 5 inch props have 14 poles. type: int32 default: 14 + min: 2 + max: 400 + num_instances: 12 + instance_start: 1