foc_simple/foc/
foc_hall_sensor.rs

1use crate::foc::EDir;
2use crate::foc::MAX_MOTOR_NR;
3use crate::EFocSimpleError;
4use crate::Result;
5use fixed::types::I16F16;
6use rtt_target::rprintln;
7
8// lookup table from the raw hall state (1-3-2-6-4-5  as index to get the hall state )
9//                                  0  1  2  3  4  5  6  7
10const HALL_STATE_TABLE: [usize; 8] = [0, 0, 2, 1, 4, 5, 3, 0];
11pub struct FocHallSensor {
12  motor_nr: usize,
13  velocity: f32, // rotation speed in rad/microsecond
14  angle: I16F16,
15  angle_per_state: I16F16, // == TAU / (nr_poles * 6)
16  dir: EDir,
17  state_prev: usize,
18  error_count: usize,
19  hall_idx_max: usize,  // nr_poles times 6
20  hall_idx_base: usize, // increments each time with 6 until the hall_idx_max
21}
22
23impl FocHallSensor {
24  pub fn new(motor_nr: usize, nr_poles: usize) -> Self {
25    let hall_idx_max = nr_poles * 6;
26    let angle_per_state = I16F16::TAU / hall_idx_max as i32;
27    if motor_nr >= MAX_MOTOR_NR {
28      rprintln!("Incorrect motor number. Panic now");
29      panic!();
30    }
31    FocHallSensor {
32      motor_nr,
33      dir: EDir::Stopped,
34      state_prev: 0,
35      error_count: 0,
36      velocity: 0.0,
37      angle: I16F16::ZERO,
38      angle_per_state,
39      hall_idx_base: 0,
40      hall_idx_max,
41    }
42  }
43
44  pub fn get_errorcount(&self) -> usize {
45    self.error_count
46  }
47  pub fn reset_errorcount(&mut self) {
48    self.error_count = 0;
49  }
50
51  /// update the angle of the shaft with the hall state
52  /// incoming raw state is the hall sensor state in the first 3 bits. Valid transitions: 1, 3, 2 ,6 ,4 ,5
53
54  pub fn update(&mut self, raw_state: usize) -> Result<I16F16> {
55    if raw_state == 0 || raw_state > 6 {
56      self.error_count += 1;
57      rprintln!("Error Invalid hall state {}", raw_state,);
58      Err(EFocSimpleError::AngleSensorError)
59    } else {
60      let prev = self.state_prev;
61      let state = HALL_STATE_TABLE[raw_state];
62      self.state_prev = state;
63
64      match state {
65        0 => {
66          if prev == 5 {
67            self.hall_idx_base += 6;
68            if self.hall_idx_base >= self.hall_idx_max {
69              self.hall_idx_base = 0;
70            }
71            self.dir = EDir::Cw;
72          } else if prev != 1 {
73            rprintln!("Error change new:{} old:{}", state, prev);
74            self.error_count += 1
75          }
76        }
77        5 => {
78          if prev == 0 {
79            if self.hall_idx_base < 6 {
80              self.hall_idx_base = self.hall_idx_max - 6;
81            } else {
82              self.hall_idx_base -= 6;
83            }
84          } else if prev != 4 {
85            rprintln!("Error change new:{} old:{}", state, prev);
86            self.error_count += 1
87          }
88        }
89        _ => {
90          if state.abs_diff(prev) != 1 {
91            rprintln!("Error change new:{} old:{}", state, prev);
92            self.error_count += 1
93          }
94        }
95      }
96      let hall_state_idx = self.hall_idx_base + state;
97      // calculate the not interpolated angle
98      let angle = self.angle_per_state * hall_state_idx as i32;
99
100      Ok(angle)
101    }
102  }
103}
foc_simple/foc/foc_hall_sensor.rs

foc_simple/foc/
foc_hall_sensor.rs
