thrust_linearization.py

class ThrustLinearization:
    def __init__(self, thrust_linear):
        self.thrustLinearization = thrust_linear / 100.0
        if self.thrustLinearization != 0.0: 
            self.thrustLinearizationReciprocal = 1.0 / self.thrustLinearization
            self.thrustLinearizationB = (1.0 - self.thrustLinearization) / (2.0 * self.thrustLinearization)
            
    def applyThrustLinearization(self, motorOutput):
        if self.thrustLinearization != 0.0:
            #if motorOutput > 0.0:
                motorOutput = np.sqrt(motorOutput * self.thrustLinearizationReciprocal +
                                self.thrustLinearizationB * self.thrustLinearizationB) - self.thrustLinearizationB

        return motorOutput;

    def pidCompensateThrustLinearization(self, throttle):
        if self.thrustLinearization != 0.0:
            throttle = throttle * (throttle * thrustLinearization + 1.0 - thrustLinearization)        
        return throttle;

            
    def showPlot(self):
        x = np.arange(0, 1.0, 0.01)
        y = self.applyThrustLinearization(x)
        #y = self.pidCompensateThrustLinearization(y)
        plt.plot(x,y, label=f'{self.thrustLinearization}')        
        plt.legend()