diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h index 246593d4a..83ff601fd 100644 --- a/Firmware/FFBoard/Inc/Axis.h +++ b/Firmware/FFBoard/Inc/Axis.h @@ -28,6 +28,7 @@ #ifdef USE_DSP_FUNCTIONS #include "dsp/controller_functions.h" #include "dsp/fast_math_functions.h" +#include "dsp/statistics_functions.h" #endif #define INTERNAL_AXIS_DAMPER_SCALER 0.7 @@ -72,6 +73,12 @@ struct AxisFlashAddresses uint16_t speedAccelFilter = ADR_AXIS1_SPEEDACCEL_FILTER; uint16_t postprocess1 = ADR_AXIS1_POSTPROCESS1; + // NOTE: The following addresses must be defined in constants.h + uint16_t equalizer1 = ADR_AXIS1_EQ1; + uint16_t equalizer2 = ADR_AXIS1_EQ2; + uint16_t equalizer3 = ADR_AXIS1_EQ3; + uint16_t handsOffConfig = ADR_AXIS1_HANDSOFF_CONF; + uint16_t handsOffAccel = ADR_AXIS1_HANDSOFF_ACCEL; }; /** @@ -120,7 +127,9 @@ enum class Axis_commands : uint32_t{ maxspeed,slewrate, calibrate_maxSlewRateDrv, maxSlewRateDrv, - expo,exposcale + expo,exposcale, + equalizer,eqb1,eqb2,eqb3,eqb4,eqb5,eqb6, + handsoff, handsoff_speed, handsoff_accel }; /** @@ -386,6 +395,12 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle const float AXIS_INERTIA_RATIO = INTERNAL_SCALER_INERTIA * INTERNAL_AXIS_INERTIA_SCALER / 255.0; // Private methods + /** + * @brief Sets the gain for a specific equalizer band. + * @param band The equalizer band index (0-4). + * @param gain The gain in 0.1 dB steps (-120 to 120). + */ + void setEqGain(uint8_t band, int8_t gain); /** * @brief Sets the degrees of rotation for the axis. * @param degrees The new range of rotation. @@ -412,7 +427,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle */ void setExpo(int val); - int32_t calculateExpoTorque(int32_t torque); + float calculateExpoTorque(float torque); /** * @brief Applies the speed limiter PI controller to the torque. * @param torque A reference to the torque value to be modified. @@ -436,6 +451,10 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle */ static uint16_t encodeConfToInt(AxisConfig conf); + /** + * @brief Checks if the user has let go of the wheel and updates the torque accordingly. + */ + void updateHandsOffState(); // Member variables @@ -525,11 +544,27 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle Biquad frictionFilter = Biquad(BiquadType::lowpass, filterFrictionCst.freq/filter_f, filterFrictionCst.q / 100.0, 0.0); Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0); + // Equalizer + static const uint8_t num_eq_bands = 6; + const std::array eq_frequencies = {10, 15, 25, 40, 60, 100}; + std::array eqFilters; + std::array eqGains = {0,0,0,0,0}; // Gains from -120 to 120, represents gain in dB * 10 + bool equalizerEnabled = false; //!< Enable/disable the equalizer. + // Post-processing GearRatio_t gearRatio; //!< Gear ratio between encoder and axis. int expoValue = 0; //!< Raw integer value for the expo curve. Formula: v = val*2 => v<0 ? 1/-v : v float expo = 1; //!< Calculated exponent for the torque curve. float expoScaler = 50; //!< Scaler for the expo calculation : 0.28 to 3.54 + + // Hands-off detection + bool handsOffCheckEnabled = false; + float handsOffAccelThreshold = 0.1; + uint16_t handsOffSpeedThreshold = 720; // deg/s + bool handsOff = false; + float accel_buffer[16] = {0}; + uint8_t accel_buffer_idx = 0; + uint32_t handsOffTimer = 0; }; #endif /* SRC_AXIS_H_ */ diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp index 187142ebc..076889865 100644 --- a/Firmware/FFBoard/Src/Axis.cpp +++ b/Firmware/FFBoard/Src/Axis.cpp @@ -119,7 +119,10 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_ setInstance(0); this->flashAddresses = AxisFlashAddresses({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL, ADR_AXIS1_MAX_SLEWRATE_DRV, ADR_AXIS1_ENDSTOP, ADR_AXIS1_POWER, ADR_AXIS1_DEGREES,ADR_AXIS1_EFFECTS1,ADR_AXIS1_EFFECTS2,ADR_AXIS1_ENC_RATIO, - ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1}); + ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1, + ADR_AXIS1_EQ1,ADR_AXIS1_EQ2,ADR_AXIS1_EQ3, + ADR_AXIS1_HANDSOFF_CONF, ADR_AXIS1_HANDSOFF_ACCEL + }); } else if (axis == 'Y') { @@ -127,20 +130,29 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_ setInstance(1); this->flashAddresses = AxisFlashAddresses({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL, ADR_AXIS2_MAX_SLEWRATE_DRV, ADR_AXIS2_ENDSTOP, ADR_AXIS2_POWER, ADR_AXIS2_DEGREES,ADR_AXIS2_EFFECTS1,ADR_AXIS2_EFFECTS2, ADR_AXIS2_ENC_RATIO, - ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1}); + ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1, + ADR_AXIS2_EQ1,ADR_AXIS2_EQ2,ADR_AXIS2_EQ3, + ADR_AXIS2_HANDSOFF_CONF, ADR_AXIS2_HANDSOFF_ACCEL + }); } else if (axis == 'Z') { setInstance(2); this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, ADR_AXIS3_MAX_SLEWRATE_DRV, ADR_AXIS3_ENDSTOP, ADR_AXIS3_POWER, ADR_AXIS3_DEGREES,ADR_AXIS3_EFFECTS1,ADR_AXIS3_EFFECTS2,ADR_AXIS3_ENC_RATIO, - ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1}); + ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1, + ADR_AXIS3_EQ1,ADR_AXIS3_EQ2,ADR_AXIS3_EQ3, + ADR_AXIS3_HANDSOFF_CONF, ADR_AXIS3_HANDSOFF_ACCEL + }); } // Initialize equalizer filters - /*for (uint8_t idx = 0; idx < num_eq_bands; idx++) { - eqFilters[idx].setBiquad(BiquadType::peak, eq_frequencies[idx] / filter_f, 1.0, 0.0); - }*/ + for (uint8_t idx = 0; idx < num_eq_bands; idx++) { + BiquadType type = BiquadType::peak; + if (idx == 0) type = BiquadType::lowshelf; + else if (idx == num_eq_bands - 1) type = BiquadType::highshelf; + eqFilters[idx].setBiquad(type, eq_frequencies[idx] / filter_f, 2.14f, 0.0); + } CommandHandler::registerCommands(); // Internal commands registerCommands(); @@ -186,6 +198,21 @@ void Axis::registerCommands(){ registerCommand("cpr", Axis_commands::cpr, "Reported encoder CPR",CMDFLAG_GET); registerCommand("expo", Axis_commands::expo, "Exponential curve correction (x^(val/exposcale)+1)", CMDFLAG_GET | CMDFLAG_SET); registerCommand("exposcale", Axis_commands::exposcale, "Scaler constant for expo", CMDFLAG_GET); + + registerCommand("equalizer", Axis_commands::equalizer, "Equalizer enable", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb1", Axis_commands::eqb1, "Equalizer band 1 gain (-120/120)", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb2", Axis_commands::eqb2, "Eq bd 2", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb3", Axis_commands::eqb3, "Eq bd 3", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb4", Axis_commands::eqb4, "Eq bd 4", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb5", Axis_commands::eqb5, "Eq bd 5", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb6", Axis_commands::eqb6, "Eq bd 6", CMDFLAG_GET | CMDFLAG_SET); + + registerCommand("handsoff", Axis_commands::handsoff, "Hands-off enable", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("handsoff_speed", Axis_commands::handsoff_speed, "Hoff speed thrld (deg/s)", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("handsoff_accel", Axis_commands::handsoff_accel, "Hoff accel std dev thrld (float, val/1000)", CMDFLAG_GET | CMDFLAG_SET); + + registerCommand("maxSlewRateDrv", Axis_commands::maxSlewRateDrv, "Max driver torque in counts/ms",CMDFLAG_GET); + registerCommand("calibrate_maxSlewRateDrv", Axis_commands::calibrate_maxSlewRateDrv, "Start driver slewRate calib", CMDFLAG_GET); } /* @@ -275,8 +302,32 @@ void Axis::restoreFlash(){ uint16_t pp1; if(Flash_Read(flashAddresses.postprocess1, &pp1)){ setExpo((int8_t)(pp1 & 0xff)); + equalizerEnabled = (pp1 >> 8) & 0x01; } + uint16_t eq_bank_1,eq_bank_2,eq_bank_3; + if(Flash_Read(flashAddresses.equalizer1, &eq_bank_1)){ + setEqGain(0, (int8_t)(eq_bank_1 & 0xff)); + setEqGain(1, (int8_t)((eq_bank_1 >> 8) & 0xff)); + } + if(Flash_Read(flashAddresses.equalizer2, &eq_bank_2)){ + setEqGain(2, (int8_t)(eq_bank_2 & 0xff)); + setEqGain(3, (int8_t)((eq_bank_2 >> 8) & 0xff)); + } + if(Flash_Read(flashAddresses.equalizer3, &eq_bank_3)){ + setEqGain(4, (int8_t)(eq_bank_3 & 0xff)); + setEqGain(5, (int8_t)((eq_bank_3 >> 8) & 0xff)); + } + + uint16_t handsOffConf; + if(Flash_Read(flashAddresses.handsOffConfig, &handsOffConf)){ + handsOffCheckEnabled = (handsOffConf >> 15) & 0x01; + handsOffSpeedThreshold = handsOffConf & 0x7FFF; + } + uint16_t handsOffAccelVal; + if(Flash_Read(flashAddresses.handsOffAccel, &handsOffAccelVal)){ + handsOffAccelThreshold = (float)handsOffAccelVal / 1000.0; + } } // Saves parameters to flash. void Axis::saveFlash(){ @@ -298,8 +349,21 @@ void Axis::saveFlash(){ Flash_Write(flashAddresses.speedAccelFilter, filterStorage); // Postprocessing - Flash_Write(flashAddresses.postprocess1, expoValue & 0xff); - + uint16_t pp1 = (expoValue & 0xff) | (equalizerEnabled << 8); + Flash_Write(flashAddresses.postprocess1, pp1); + + // Equalizer gains + uint16_t eq1 = (eqGains[0] & 0xff) | ((eqGains[1] & 0xff) << 8); + uint16_t eq2 = (eqGains[2] & 0xff) | ((eqGains[3] & 0xff) << 8); + uint16_t eq3 = (eqGains[4] & 0xff) | ((eqGains[5] & 0xff) << 8); + Flash_Write(flashAddresses.equalizer1, eq1); + Flash_Write(flashAddresses.equalizer2, eq2); + Flash_Write(flashAddresses.equalizer3, eq3); + + // Hands-off detection + uint16_t handsOffConf = (handsOffSpeedThreshold & 0x7FFF) | (handsOffCheckEnabled << 15); + Flash_Write(flashAddresses.handsOffConfig, handsOffConf); + Flash_Write(flashAddresses.handsOffAccel, (uint16_t)(handsOffAccelThreshold * 1000.0)); } @@ -385,9 +449,31 @@ void Axis::prepareForUpdate(){ startForceFadeIn(0, 1.0); } + // Check for pending slew rate calibration result + if(this->awaitingSlewCalibration){ + // If driver reports calibration finished, retrieve measured value and persist + if(!drv->isSlewRateCalibrationInProgress()){ + // Get value from drv + this->maxSlewRate_Driver = drv->getDrvSlewRate(); + + // If the driver's max slew rate is lowest thant current max flew rate, cap the value and send the new value to the UI + if (this->maxSlewRate_Driver < this->maxTorqueRateMS) { + this->maxTorqueRateMS = this->maxSlewRate_Driver; + CommandHandler::broadcastCommandReply(CommandReply(this->maxTorqueRateMS), (uint32_t)Axis_commands::slewrate, CMDtype::get); + } + + // Broadcast a friendly completion message and the numeric value + CommandHandler::broadcastCommandReply(CommandReply("Slew rate calibration complete",0), (uint32_t)Axis_commands::calibrate_maxSlewRateDrv, CMDtype::get); + CommandHandler::broadcastCommandReply(CommandReply(this->maxSlewRate_Driver), (uint32_t)Axis_commands::maxSlewRateDrv, CMDtype::get); + + + this->awaitingSlewCalibration = false; + } + } + this->updateMetrics(angle); - //this->updateHandsOffState(); + this->updateHandsOffState(); } @@ -683,6 +769,9 @@ void Axis::updateMetrics(float new_pos) { // pos is degrees metric.current.accel = accelFilter.process((currentSpeed - previousFrameSpeed))* this->filter_f; // deg/s/s previousFrameSpeed = currentSpeed; + // Update hands-off detection buffer + accel_buffer[accel_buffer_idx] = metric.current.accel; + accel_buffer_idx = (accel_buffer_idx + 1) % (sizeof(accel_buffer) / sizeof(accel_buffer[0])); } @@ -694,35 +783,34 @@ uint16_t Axis::getPower(){ /** * Calculates an exponential torque correction curve and scale for FFBEffect */ -int32_t Axis::calculateExpoTorque(int32_t torque){ - float torquef = (float)torque / (float)0x7fff; // This down and upscaling may introduce float artifacts. Do this before scaling down. - if(torquef < 0){ - return -powf(-torquef,expo) * 0x7fff; - }else{ - return powf(torquef,expo) * 0x7fff; - } -} - -int32_t Axis::calculateFFBTorque() { +int32_t Axis::calculateFFBTorque(){ - int32_t torque = this->ffbEffectTorque; + // 1. Initial conversion and normalization + // Convert to float and move to [-1.0, 1.0] range immediately. + float torque = (float)this->ffbEffectTorque * (1.0f / 32767.0f); - // 1. Game Clipping detection - // If the game sends more than the theoretical maximum (+/- 32767), the signal is clipped at the source. - if(abs(torque) >= 0x7fff){ + // 2. Game Clipping detection + // Performed on normalized value for consistency. + if(fabsf(torque) >= 1.0f){ pulseClipLed(); // Visual alert: game signal is clipping } - // 2. Apply Expo (Linearization or sensation curve) - if(expo != 1){ - torque = calculateExpoTorque(torque); + // 3. Apply equalizer + // Standardized DSP processing on [-1.0, 1.0] range. + if(equalizerEnabled){ + for (uint8_t i = 0; i < num_eq_bands; ++i) { + torque = eqFilters[i].process(torque); + } } - // 3. Game specific gain (effectRatioScaler) - // Scale the FFB from game only (allows lowering game effects without lowering endstops) - torque = (int32_t)((float)torque * effectRatioScaler); + // 4. Apply Expo (Linearization or sensation curve) + if(expo != 1.0f){ + torque = calculateExpoTorque(torque); + } - return torque; + // 5. Final scaling and single conversion back to integer + // Apply both the FFB range and the effect ratio in one final step. + return (int32_t)(torque * 32767.0f * effectRatioScaler); } int32_t Axis::getTorque() { return metric.current.torque; } // Fix: move from previous to current @@ -831,7 +919,7 @@ void Axis::applyTorqueSlewRateLimiter(int32_t& torque) // The torque is clipped to be within the range of [previous torque - limit, previous torque + limit]. torque = clip(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange); - } +} int32_t Axis::applySpeedLimiterTorque(int32_t& torque){ // Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS. @@ -843,6 +931,7 @@ int32_t Axis::applySpeedLimiterTorque(int32_t& torque){ } int32_t resultTorque = 0; + float effectiveSpeed = metric.current.speed * (torque > 0 ? 1.0f : -1.0f); if (effectiveSpeed > maxSpeedDegS) @@ -864,9 +953,9 @@ int32_t Axis::applySpeedLimiterTorque(int32_t& torque){ // We must only reduce the magnitude of the torque, not invert it. reductionAmount = clip(reductionAmount, 0.0f, fabsf((float)torque)); if(torque > 0) { - resultTorque = reductionAmount; + resultTorque = (int32_t)reductionAmount; } else { - resultTorque = -reductionAmount; + resultTorque = (int32_t)-reductionAmount; } } else { #ifdef USE_DSP_FUNCTIONS @@ -879,9 +968,17 @@ int32_t Axis::applySpeedLimiterTorque(int32_t& torque){ return resultTorque; } +/** + * Called when the update rate changes. + * Recalculate all filters. + */ void Axis::updateSamplerate(float newSamplerate){ this->filter_f = newSamplerate; this->updateFilters(this->filterProfileId); // Recalculate filters + // Update EQ samplerate + for(uint8_t idx = 0; idx < eqFilters.size(); idx++){ + eqFilters[idx].setFc(eq_frequencies[idx] / filter_f); + } } void Axis::updateFilters(uint8_t profileId){ @@ -934,6 +1031,12 @@ void Axis::setExpo(int val){ } } +float Axis::calculateExpoTorque(float torque){ + float torqueSign = torque > 0 ? 1.0f : -1.0f; + torque = powf(fabsf(torque), expo) * torqueSign; + return torque; +} + CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& replies){ switch(static_cast(cmd.cmdId)){ @@ -950,7 +1053,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& break; case Axis_commands::degrees: - handleGetSetFunc(cmd, replies, degreesOfRotation, &Axis::setDegrees,this); + handleGetSetFunc(cmd, replies, degreesOfRotation, &Axis::setDegrees,this); break; case Axis_commands::esgain: @@ -1175,13 +1278,118 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& handleGetSet(cmd, replies, expoScaler); // need to also provide the expoScaler constant break; + case Axis_commands::equalizer: + handleGetSet(cmd, replies, this->equalizerEnabled); + break; + + case Axis_commands::eqb1: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[0]); } + else if(cmd.type == CMDtype::set){ setEqGain(0, cmd.val); } + break; + case Axis_commands::eqb2: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[1]); } + else if(cmd.type == CMDtype::set){ setEqGain(1, cmd.val); } + break; + case Axis_commands::eqb3: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[2]); } + else if(cmd.type == CMDtype::set){ setEqGain(2, cmd.val); } + break; + case Axis_commands::eqb4: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[3]); } + else if(cmd.type == CMDtype::set){ setEqGain(3, cmd.val); } + break; + case Axis_commands::eqb5: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[4]); } + else if(cmd.type == CMDtype::set){ setEqGain(4, cmd.val); } + break; + case Axis_commands::eqb6: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[5]); } + else if(cmd.type == CMDtype::set){ setEqGain(5, cmd.val); } + break; + + case Axis_commands::handsoff: + handleGetSet(cmd, replies, this->handsOffCheckEnabled); + break; + + case Axis_commands::handsoff_speed: + handleGetSet(cmd, replies, this->handsOffSpeedThreshold); + break; + + case Axis_commands::handsoff_accel: + if (cmd.type == CMDtype::get) { + replies.emplace_back(this->handsOffAccelThreshold * 1000); + } else if (cmd.type == CMDtype::set) { + this->handsOffAccelThreshold = (float)cmd.val / 1000.0; + } + break; + default: return CommandStatus::NOT_FOUND; } return CommandStatus::OK; } +void Axis::updateHandsOffState() { + if (!handsOffCheckEnabled) { + handsOff = false; + return; + } + // Downsample the check to 100Hz + static uint8_t check_counter = 0; + if(check_counter++ < 10){ + return; + } + check_counter = 0; + + + // Safety override: if speed is very high, cut torque + if (abs(metric.current.speed) > handsOffSpeedThreshold) { + handsOff = true; + return; + } + + // Main check: only run if FFB torque from game is active + const int32_t MIN_TORQUE_THRESHOLD = 1000; + if (abs(ffbEffectTorque) > MIN_TORQUE_THRESHOLD) { + // Calculate standard deviation of acceleration buffer + float std_dev = 0.0; + uint16_t buf_size = sizeof(accel_buffer) / sizeof(accel_buffer[0]); +#ifdef USE_DSP_FUNCTIONS + arm_std_f32(accel_buffer, buf_size, &std_dev); +#else + // Simple std dev calculation + float mean = 0.0; + for (uint16_t i = 0; i < buf_size; i++) { + mean += accel_buffer[i]; + } + mean /= (float)buf_size; + + float variance = 0.0; + for (uint16_t i = 0; i < buf_size; i++) { + variance += (accel_buffer[i] - mean) * (accel_buffer[i] - mean); + } + variance /= (float)buf_size; + std_dev = sqrtf(variance); +#endif + + + if (std_dev < handsOffAccelThreshold) { + if (handsOffTimer < 50) { // 50 * 10ms = 500ms timeout + handsOffTimer++; + } else { + handsOff = true; + } + } else { + handsOffTimer = 0; + handsOff = false; + } + } else { + // Torque is low, assume hands are on + handsOffTimer = 0; + handsOff = false; + } +} /* * Helper functions for encoding and decoding flash variables @@ -1201,3 +1409,10 @@ uint16_t Axis::encodeConfToInt(AxisConfig conf) val |= ((uint8_t)conf.drvtype & 0x3f) << 6; return val; } + +void Axis::setEqGain(uint8_t band, int8_t gain) { + if (band >= num_eq_bands) return; + gain = clip(gain, -120, 120); + eqGains[band] = gain; + eqFilters[band].setPeakGain(gain / 10.0); +} diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h index bed80e3af..6c70a395f 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h +++ b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h @@ -13,11 +13,11 @@ #include "main.h" // Change this to the amount of currently registered variables -#define NB_OF_VAR 168 +#define NB_OF_VAR 183 extern const uint16_t VirtAddVarTab[NB_OF_VAR]; // Amount of variables in exportable list -#define NB_EXPORTABLE_ADR 153 +#define NB_EXPORTABLE_ADR 168 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR]; @@ -106,6 +106,11 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS1_ENC_RATIO 0x30A // Accel filter Lowpass #define ADR_AXIS1_EFFECTS2 0x30B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS1_POSTPROCESS1 0x30C // 0-7 expo curve +#define ADR_AXIS1_EQ1 0x30D // Equalizer band 1,2 gains +#define ADR_AXIS1_EQ2 0x30E // Equalizer band 3,4 gains +#define ADR_AXIS1_EQ3 0x30F // Equalizer band 5 gain +#define ADR_AXIS1_HANDSOFF_CONF 0x310 // Hands-off config +#define ADR_AXIS1_HANDSOFF_ACCEL 0x311 // Hands-off accel threshold // TMC1 #define ADR_TMC1_MOTCONF 0x320 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC1_CPR 0x321 @@ -133,6 +138,11 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS2_ENC_RATIO 0x34A // Store the encoder ratio for an axis #define ADR_AXIS2_EFFECTS2 0x34B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS2_POSTPROCESS1 0x34C // 0-7 expo curve +#define ADR_AXIS2_EQ1 0x34D // Equalizer band 1,2 gains +#define ADR_AXIS2_EQ2 0x34E // Equalizer band 3,4 gains +#define ADR_AXIS2_EQ3 0x34F // Equalizer band 5 gain +#define ADR_AXIS2_HANDSOFF_CONF 0x350 // Hands-off config +#define ADR_AXIS2_HANDSOFF_ACCEL 0x351 // Hands-off accel threshold // TMC2 #define ADR_TMC2_MOTCONF 0x360 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC2_CPR 0x361 @@ -160,6 +170,11 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS3_ENC_RATIO 0x38A // Store the encoder ratio for an axis #define ADR_AXIS3_EFFECTS2 0x38B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS3_POSTPROCESS1 0x38C // 0-7 expo curve +#define ADR_AXIS3_EQ1 0x38D // Equalizer band 1,2 gains +#define ADR_AXIS3_EQ2 0x38E // Equalizer band 3,4 gains +#define ADR_AXIS3_EQ3 0x38F // Equalizer band 5 gain +#define ADR_AXIS3_HANDSOFF_CONF 0x390 // Hands-off config +#define ADR_AXIS3_HANDSOFF_ACCEL 0x391 // Hands-off accel threshold // TMC3 #define ADR_TMC3_MOTCONF 0x3A0 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC3_CPR 0x3A1 diff --git a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c index 2a0e83a21..acca3df72 100644 --- a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c +++ b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c @@ -88,6 +88,11 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_AXIS1_ENC_RATIO, // Accel filter Lowpass ADR_AXIS1_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS1_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS1_EQ1, // Equalizer band 1,2 gains + ADR_AXIS1_EQ2, // Equalizer band 3,4 gains + ADR_AXIS1_EQ3, // Equalizer band 5 gain + ADR_AXIS1_HANDSOFF_CONF, // Hands-off config + ADR_AXIS1_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC1 ADR_TMC1_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC1_CPR, @@ -115,6 +120,11 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_AXIS2_ENC_RATIO, // Store the encoder ratio for an axis ADR_AXIS2_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS2_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS2_EQ1, // Equalizer band 1,2 gains + ADR_AXIS2_EQ2, // Equalizer band 3,4 gains + ADR_AXIS2_EQ3, // Equalizer band 5 gain + ADR_AXIS2_HANDSOFF_CONF, // Hands-off config + ADR_AXIS2_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC2 ADR_TMC2_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC2_CPR, @@ -142,6 +152,11 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_AXIS3_ENC_RATIO, // Store the encoder ratio for an axis ADR_AXIS3_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS3_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS3_EQ1, // Equalizer band 1,2 gains + ADR_AXIS3_EQ2, // Equalizer band 3,4 gains + ADR_AXIS3_EQ3, // Equalizer band 5 gain + ADR_AXIS3_HANDSOFF_CONF, // Hands-off config + ADR_AXIS3_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC3 ADR_TMC3_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC3_CPR, @@ -292,6 +307,11 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_AXIS1_ENC_RATIO, // Accel filter Lowpass ADR_AXIS1_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS1_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS1_EQ1, // Equalizer band 1,2 gains + ADR_AXIS1_EQ2, // Equalizer band 3,4 gains + ADR_AXIS1_EQ3, // Equalizer band 5 gain + ADR_AXIS1_HANDSOFF_CONF, // Hands-off config + ADR_AXIS1_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC1 ADR_TMC1_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC1_CPR, @@ -319,6 +339,11 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_AXIS2_ENC_RATIO, // Store the encoder ratio for an axis ADR_AXIS2_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS2_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS2_EQ1, // Equalizer band 1,2 gains + ADR_AXIS2_EQ2, // Equalizer band 3,4 gains + ADR_AXIS2_EQ3, // Equalizer band 5 gain + ADR_AXIS2_HANDSOFF_CONF, // Hands-off config + ADR_AXIS2_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC2 ADR_TMC2_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC2_CPR, @@ -346,6 +371,11 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_AXIS3_ENC_RATIO, // Store the encoder ratio for an axis ADR_AXIS3_EFFECTS2, // 0-7 Friction, 8-15 Inertia ADR_AXIS3_POSTPROCESS1, // 0-7 expo curve + ADR_AXIS3_EQ1, // Equalizer band 1,2 gains + ADR_AXIS3_EQ2, // Equalizer band 3,4 gains + ADR_AXIS3_EQ3, // Equalizer band 5 gain + ADR_AXIS3_HANDSOFF_CONF, // Hands-off config + ADR_AXIS3_HANDSOFF_ACCEL, // Hands-off accel threshold // TMC3 ADR_TMC3_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles ADR_TMC3_CPR, diff --git a/Firmware/Makefile b/Firmware/Makefile index a3b53944a..42b43a702 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -170,10 +170,13 @@ C_SOURCES += $(wildcard $(DSP_LIB_PATH)/Source/BasicMathFunctions/arm_*.c) \ $(wildcard $(DSP_LIB_PATH)/Source/FastMathFunctions/arm_*.c) \ $(wildcard $(DSP_LIB_PATH)/Source/FilteringFunctions/arm_*.c) \ $(wildcard $(DSP_LIB_PATH)/Source/ControllerFunctions/arm_*.c) \ - $(wildcard $(DSP_LIB_PATH)/Source/InterpolationFunctions/arm_*.c) + $(wildcard $(DSP_LIB_PATH)/Source/InterpolationFunctions/arm_*.c) \ + $(wildcard $(DSP_LIB_PATH)/Source/StatisticsFunctions/arm_*.c) # Filter out unnecessary or incompatible files C_SOURCES := $(filter-out %_f16.c %_f64.c %_mve.c, $(C_SOURCES)) + + endif # compile gcc flags @@ -203,7 +206,6 @@ endif # libraries LIBS = -lc -lm -lnosys -LIBDIR = LDFLAGS = $(MCU) --specs=standard_c_nano_cpp.specs -u _printf_float -Wl,--start-group -lc -lm -lstdc++ -lsupc++ -Wl,--end-group -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections -static # -mcpu=cortex-m4 -T"C:\Users\zhouli\Desktop\my-openffb\OpenFFBoard\Firmware\Targets\F407VG\STM32F407VGTX_FLASH.ld" --specs=nosys.specs -Wl,-Map="OpenFFBoard_F407VG.map" -Wl,--cref -Wl,--gc-sections -static --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -u _printf_float -Wl,--start-group -lc -lm -lstdc++ -lsupc++ -Wl,--end-group diff --git a/Firmware/scripts/memory_map.csv b/Firmware/scripts/memory_map.csv index 822ba9c93..ab47a985b 100644 --- a/Firmware/scripts/memory_map.csv +++ b/Firmware/scripts/memory_map.csv @@ -70,6 +70,11 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_AXIS1_ENC_RATIO,0x30A,// Accel filter Lowpass,1,1 ,ADR_AXIS1_EFFECTS2,0x30B,"// 0-7 Friction, 8-15 Inertia",1,1 ,ADR_AXIS1_POSTPROCESS1,0x30C,// 0-7 expo curve,1,1 +,ADR_AXIS1_EQ1,0x30D,"// Equalizer band 1,2 gains",1,1 +,ADR_AXIS1_EQ2,0x30E,"// Equalizer band 3,4 gains",1,1 +,ADR_AXIS1_EQ3,0x30F,"// Equalizer band 5 gain",1,1 +,ADR_AXIS1_HANDSOFF_CONF,0x310,"// Hands-off config",1,1 +,ADR_AXIS1_HANDSOFF_ACCEL,0x311,"// Hands-off accel threshold",1,1 // TMC1,,,,, ,ADR_TMC1_MOTCONF,0x320,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1 ,ADR_TMC1_CPR,0x321,,1,1 @@ -97,12 +102,11 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_AXIS2_ENC_RATIO,0x34A,// Store the encoder ratio for an axis,1,1 ,ADR_AXIS2_EFFECTS2,0x34B,"// 0-7 Friction, 8-15 Inertia",1,1 ,ADR_AXIS2_POSTPROCESS1,0x34C,// 0-7 expo curve,1,1 -,,,,, -,,,,, -,,,,, -,,,,, -,,,,, -,,,,, +,ADR_AXIS2_EQ1,0x34D,"// Equalizer band 1,2 gains",1,1 +,ADR_AXIS2_EQ2,0x34E,"// Equalizer band 3,4 gains",1,1 +,ADR_AXIS2_EQ3,0x34F,"// Equalizer band 5 gain",1,1 +,ADR_AXIS2_HANDSOFF_CONF,0x350,"// Hands-off config",1,1 +,ADR_AXIS2_HANDSOFF_ACCEL,0x351,"// Hands-off accel threshold",1,1 // TMC2,,,,, ,ADR_TMC2_MOTCONF,0x360,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1 ,ADR_TMC2_CPR,0x361,,1,1 @@ -130,8 +134,11 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_AXIS3_ENC_RATIO,0x38A,// Store the encoder ratio for an axis,1,1 ,ADR_AXIS3_EFFECTS2,0x38B,"// 0-7 Friction, 8-15 Inertia",1,1 ,ADR_AXIS3_POSTPROCESS1,0x38C,// 0-7 expo curve,1,1 -,,,,, -,,,,, +,ADR_AXIS3_EQ1,0x38D,"// Equalizer band 1,2 gains",1,1 +,ADR_AXIS3_EQ2,0x38E,"// Equalizer band 3,4 gains",1,1 +,ADR_AXIS3_EQ3,0x38F,"// Equalizer band 5 gain",1,1 +,ADR_AXIS3_HANDSOFF_CONF,0x390,"// Hands-off config",1,1 +,ADR_AXIS3_HANDSOFF_ACCEL,0x391,"// Hands-off accel threshold",1,1 // TMC3,,,,, ,ADR_TMC3_MOTCONF,0x3A0,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1 ,ADR_TMC3_CPR,0x3A1,,1,1