Skip to content
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

Added TI angle observer code, and related parameters #9

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Added TI angle observer code, and related parameters #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3bb15b2
Added TI angle observer code, and related parameters
mjc-506 Apr 19, 2021
937a9f0
Use speed output from TI observer code, instead of turns/time
mjc-506 Apr 19, 2021
cf56b8d
typo...
mjc-506 Apr 20, 2021
6f8cac4
Corrected scaling
mjc-506 Apr 20, 2021
555d9db
Remove binary... sigh
mjc-506 Apr 20, 2021
e80fcec
Correct runaway on braking - 'speed' must remain +ve
mjc-506 Apr 20, 2021
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
12 changes: 9 additions & 3 deletions include/param_prj.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,22 +17,25 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#define VER 5.03.R
#define VER 5.03.R.TIobs_test

/* Entries must be ordered as follows:
1. Saveable parameters (id != 0)
2. Temporary parameters (id = 0)
3. Display values
*/
//Next param id (increase when adding new parameter!): 132
//Next value Id: 2048
//Next param id (increase when adding new parameter!): 135
//Next value Id: 2050
/* category name unit min max default id */

#define MOTOR_PARAMETERS_COMMON \
PARAM_ENTRY(CAT_MOTOR, polepairs, "", 1, 16, 2, 32 ) \
PARAM_ENTRY(CAT_MOTOR, respolepairs,"", 1, 16, 1, 93 ) \
PARAM_ENTRY(CAT_MOTOR, sincosofs, "dig", 1, 4096, 2048, 131 ) \
PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 5, 0, 75 ) \
PARAM_ENTRY(CAT_MOTOR, anglemode, ANGMODES, 0, 1, 0, 132 ) \
PARAM_ENTRY(CAT_MOTOR, encK1, "dig" , 0, 100000, 550, 133 ) \
PARAM_ENTRY(CAT_MOTOR, encK2, "dig", 0, 100000, 80000, 134 ) \
PARAM_ENTRY(CAT_MOTOR, fmax, "Hz", 21, 1000, 200, 9 ) \
PARAM_ENTRY(CAT_MOTOR, numimp, "ppr", 8, 8192, 60, 15 ) \
PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 20000, 100, 87 ) \
Expand Down Expand Up @@ -157,6 +160,8 @@
VALUE_ENTRY(turns, "", 2037 ) \
VALUE_ENTRY(amp, "dig", 2013 ) \
VALUE_ENTRY(angle, "°", 2014 ) \
VALUE_ENTRY(angleatan, "°", 2048 ) \
VALUE_ENTRY(angleobs, "°", 2049 ) \
VALUE_ENTRY(pot, "dig", 2015 ) \
VALUE_ENTRY(pot2, "dig", 2016 ) \
VALUE_ENTRY(potnom, "%", 2017 ) \
Expand Down Expand Up @@ -248,6 +253,7 @@
#define OKERR "0=Error, 1=Ok, 2=na"
#define CHARGEMODS "0=Off, 3=Boost, 4=Buck"
#define ENCMODES "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver, 5=SinCos"
#define ANGMODES "0=atan2, 1=TIobs"
#define POTMODES "0=SingleRegen, 1=DualChannel, 2=CAN, 3=CANDual"
#define CANSPEEDS "0=250k, 1=500k, 2=800k, 3=1M"
#define CANIOS "1=Cruise, 2=Start, 4=Brake, 8=Fwd, 16=Rev, 32=Bms"
Expand Down
41 changes: 38 additions & 3 deletions src/inc_encoder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,10 @@
#define FRQ_TO_PSC(frq) ((72000000 / frq) - 1)
#define NUM_ENCODER_CONFIGS (sizeof(encoderConfigurations) / sizeof(encoderConfigurations[0]))

//TI encoder observer stuff
static volatile int32_t integrator1 = 0;
static volatile int32_t integrator2 = 0;

typedef struct EncoderConfiguration
{
uint32_t pulseMeasFrequency;
Expand Down Expand Up @@ -249,7 +253,10 @@ void Encoder::UpdateRotorFrequency(int callingFrequency)
int absTurns = ABS(turnsSinceLastSample);
if (startupDelay == 0 && absTurns > STABLE_ANGLE)
{
lastFrequency = (callingFrequency * absTurns) / FP_TOINT(TWO_PI);
if (encMode == SPI)
{
lastFrequency = (callingFrequency * absTurns) / FP_TOINT(TWO_PI);
}
detectedDirection = turnsSinceLastSample > 0 ? 1 : -1;
}
else
Expand Down Expand Up @@ -556,8 +563,36 @@ uint16_t Encoder::DecodeAngle(bool invert)
if ((resolverMax - resolverMin) > MIN_RES_AMP)
{
if (invert)
return SineCore::Atan2(-sin, -cos);
return SineCore::Atan2(sin, cos);
{
sin = -sin;
cos = -cos;
}
uint16_t angleatan = SineCore::Atan2(sin, cos);
Param::SetFlt(Param::angleatan, FP_FROMINT(angleatan) / (65536 / 360)); //original atan2 angle calculation

//TI observer based implementation
uint16_t K1 = Param::GetInt(Param::encK1); //probably doesn't need to be a parameter.
uint16_t K2 = Param::GetInt(Param::encK2); //likewise
uint16_t frequency = 8000;// hz. Could be set dynamically if needed, but seems to work ok static.
int32_t cos_sin_anglast = cos*SineCore::Sine(integrator2)/32767; //angle_last = integrator2
int32_t sin_cos_anglast = sin*SineCore::Cosine(integrator2)/32767;
int32_t sum_one = sin_cos_anglast - cos_sin_anglast;
integrator1 += sum_one*K2/frequency;
int32_t gain1 = sum_one*K1;
int32_t sum_two = integrator1 + gain1;
integrator2 += sum_two/frequency;
int32_t sample_delay_comp = integrator1/(2*frequency);
uint16_t obs_angle = sample_delay_comp-integrator2+16384; //digits, 2pi rad = 360 deg = 65536 digits. Offset and -ve to match original atan2 code - prevents need to reset encoffset
Param::SetFlt(Param::angleobs, FP_FROMINT(obs_angle) / (65536 / 360));
lastFrequency = ABS(FP_FROMINT(integrator1)/TWO_PI);
if (Param::GetInt(Param::anglemode) == 1) //TI observer mode
{
return obs_angle;
}
else //original atan2 mode
{
return angleatan;
}
}
else
{
Expand Down