Bayley Wang / Mbed 2 deprecated priustroller

Prius IPM controller

Dependencies:   mbed

Fork of analoghalls5_5 by N K

sensors/positionsensors.cpp@29:cb03760ba9ea, 2015-03-14 (annotated)

Committer:
nki
Date:
Sat Mar 14 23:42:46 2015 +0000
Revision:
29:cb03760ba9ea
Parent:
28:af9d43bcb371
Child:
33:e7b132029bae
Cautionary commit, possibly works

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 11:dccbaa9274c5 1 #include "includes.h"
bwang 11:dccbaa9274c5 2 #include "sensors.h"
bwang 11:dccbaa9274c5 3 #include "lut.h"
bwang 11:dccbaa9274c5 4
bwang 11:dccbaa9274c5 5 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a,
bwang 11:dccbaa9274c5 6 float cal1_b, float cal2_b, float offset) {
bwang 28:af9d43bcb371 7 _in_a = new NativeAnalogIn(pin_a);
bwang 28:af9d43bcb371 8 _in_b = new NativeAnalogIn(pin_b);
bwang 11:dccbaa9274c5 9 _cal1_a = cal1_a;
bwang 11:dccbaa9274c5 10 _cal2_a = cal2_a;
bwang 11:dccbaa9274c5 11 _cal1_b = cal1_b;
bwang 11:dccbaa9274c5 12 _cal2_b = cal2_b;
bwang 11:dccbaa9274c5 13 _offset = offset;
bwang 11:dccbaa9274c5 14 }
bwang 11:dccbaa9274c5 15
bwang 11:dccbaa9274c5 16 float AnalogHallPositionSensor::GetPosition() {
bwang 11:dccbaa9274c5 17 float angle = 0.0f;
bwang 11:dccbaa9274c5 18 float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
bwang 11:dccbaa9274c5 19 float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
bwang 11:dccbaa9274c5 20
nki 29:cb03760ba9ea 21 dbg_ascaled = ascaled;
nki 29:cb03760ba9ea 22 dbg_bscaled = bscaled;
nki 29:cb03760ba9ea 23
bwang 11:dccbaa9274c5 24 float x = bscaled / ascaled;
bwang 11:dccbaa9274c5 25
bwang 11:dccbaa9274c5 26 unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
bwang 11:dccbaa9274c5 27
bwang 11:dccbaa9274c5 28 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 11:dccbaa9274c5 29
bwang 11:dccbaa9274c5 30 if(ascaled < 0){
bwang 11:dccbaa9274c5 31 if(bscaled < 0) angle = 90 - arctan[index];
bwang 11:dccbaa9274c5 32 if(bscaled > 0) angle = 90 + arctan[index];
bwang 11:dccbaa9274c5 33 }
bwang 11:dccbaa9274c5 34
bwang 11:dccbaa9274c5 35 if(ascaled>0){
bwang 11:dccbaa9274c5 36 if(bscaled > 0) angle = 270 - arctan[index];
bwang 11:dccbaa9274c5 37 if(bscaled < 0) angle = 270 + arctan[index];
bwang 11:dccbaa9274c5 38 }
bwang 11:dccbaa9274c5 39
bwang 11:dccbaa9274c5 40 if(angle > 360.0f) angle = 360.0f;
bwang 11:dccbaa9274c5 41 if(angle < 0) angle = 0;
bwang 11:dccbaa9274c5 42
bwang 11:dccbaa9274c5 43 angle -= _offset;
bwang 11:dccbaa9274c5 44 if (angle < 0.0f) angle += 360.0f;
bwang 11:dccbaa9274c5 45 if (angle >= 360.0f) angle -= 360.0f;
bwang 11:dccbaa9274c5 46
bwang 11:dccbaa9274c5 47 return angle;
bwang 11:dccbaa9274c5 48 }