MotorWare f2806x Module API Documentation

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

enc.c
Go to the documentation of this file.
1 /* --COPYRIGHT--,BSD
2 * Copyright (c) 2012, Texas Instruments Incorporated
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 *
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * * Neither the name of Texas Instruments Incorporated nor the names of
17 * its contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 * --/COPYRIGHT--*/
37
38
39 // **************************************************************************
40 // the includes
41
42 #include "sw/modules/enc/src/32b/enc.h"
43
44
45 // **************************************************************************
46 // the defines
47
48
49 // **************************************************************************
50 // the globals
51
52 // **************************************************************************
53 // the functions
54
55 ENC_Handle ENC_init(void *pMemory,const size_t numBytes)
56 {
57 ENC_Handle encHandle;
58
59 if(numBytes < sizeof(ENC_Obj))
60 return((ENC_Handle)NULL);
61
62 // assign the handle
63 encHandle = (ENC_Handle)pMemory;
64
65 return(encHandle);
66 } // end of ENC_init() function
67
68 void ENC_setup(ENC_Handle encHandle, const int16_t sample_period, const uint16_t num_pole_pairs, const uint16_t num_enc_slots, const uint32_t enc_zero_offset, const float_t full_scale_freq, const float_t speed_update_freq, const float_t speed_cutoff)
69 {
70 ENC_Obj *enc;
71 float_t temp;
72 float_t speed_cutoff_radians;
73 int16_t i;
74
75 // create an object pointer for manipulation
76 enc = (ENC_Obj *)encHandle;
77
78 // setup the encoder sample rate
79 enc->sample_count = 0;
80 enc->sample_period = sample_period;
81
82 // copy the parameters into the data structure
83 enc->num_enc_slots = num_enc_slots;
84 enc->num_pole_pairs = num_pole_pairs;
85 enc->enc_zero_offset = enc_zero_offset;
86 enc->full_scale_freq = full_scale_freq;
87 enc->delta_enc = 0;
88 enc->prev_enc = 0;
89
90 // initialize the electrical angle
91 enc->enc_elec_angle = 0;
92
93 // compute the gain which translates the mech into the elec angle
94 enc->mech_angle_gain = (_iq)((((uint32_t)1)<<24)/(4*num_enc_slots));
95
96 // compute the speed gain
97 temp = ((float_t)num_pole_pairs*speed_update_freq*ENC_SPEED_SCALING_FACTOR) / (4.0*(float_t)num_enc_slots*full_scale_freq*(float_t)sample_period);
98 enc->speed_gain = (int32_t) temp;
99
100 // compute the rpm gain
101 temp = (float_t)((full_scale_freq*60.0)/num_pole_pairs);
102 enc->rpm_gain = (int32_t) temp;
103
104 // compute the speed coefficients and initialize the low-pass filtered output
105 enc->speed_cutoff = speed_cutoff;
106 speed_cutoff_radians = ENC_2PI*speed_cutoff;
107 temp = speed_cutoff_radians/(speed_update_freq+speed_cutoff_radians);
108 enc->speed_lpf_cx = (int32_t) (ENC_SPEED_COEFF_SCALING*temp);
109 enc->speed_lpf_cy = ENC_SPEED_COEFF_SCALING - enc->speed_lpf_cx;
110 enc->speed_lpf_out = 0;
111
112 // setup the encoder log
113 enc->log_state = ENC_LOG_STATE_IDLE;
114 enc->run_flag = 0;
115 enc->trigger_idx = 0;
116 enc->trigger_delta = 0;
117 enc->log_idx = 0;
118 enc->post_trigger_len = ENC_LOG_LEN>>1;
119 enc->post_trigger_cnt = enc->post_trigger_len;
120 for (i=0; i<ENC_LOG_LEN; i++)
121 {
122 enc->log[i] = 0;
123 }
124
125 return;
126 } // end of ENC setup function
127
128
129 void ENC_calcElecAngle(ENC_Handle encHandle, uint32_t posnCounts)
130 {
131 ENC_Obj *enc;
132 uint32_t temp;
133
134 // create an object pointer for manipulation
135 enc = (ENC_Obj *) encHandle;
136
137 // compute the mechanical angle
138 temp = posnCounts*enc->mech_angle_gain;
139 // add in calibrated offset
140 temp += enc->enc_zero_offset;
141 // convert to electrical angle
142 temp = temp * enc->num_pole_pairs;
143 // wrap around 1.0 (Q24)
144 temp &= ((uint32_t) 0x00ffffff);
145 // store encoder electrical angle
146 enc->enc_elec_angle = (_iq)temp;
147 // update the slip angle
148 enc->enc_slip_angle = enc->enc_slip_angle + enc->incremental_slip;
149 // wrap around 1.0 (Q24)
150 enc->enc_slip_angle &= ((uint32_t) 0x00ffffff);
151 // add in compensation for slip
152 temp = temp + enc->enc_slip_angle;
153 // wrap around 1.0 (Q24)
154 temp &= ((uint32_t) 0x00ffffff);
155 // store encoder magnetic angle
156 enc->enc_magnetic_angle = (_iq)temp;
157
158 return;
159 } // end of ENC_calc_elec_angle() function
160
161
162
163 void ENC_run(ENC_Handle encHandle, uint32_t posnCounts, uint16_t indextFlag, uint16_t dirFlag, int16_t log_flag)
164 {
165 uint16_t dir;
166 uint16_t index_event_before;
167 uint16_t index_event_after;
168 ENC_Obj *enc;
169 int32_t enc_val;
170 int32_t delta_enc;
171 int32_t temp;
172 int16_t sample_count;
173
174 // create an object pointer for manipulation
175 enc = (ENC_Obj *) encHandle;
176
177 // update the encoder counter
178 sample_count = enc->sample_count;
179 sample_count++;
180
181 // if it reaches the sample period, read and process encoder data
182 if (sample_count == enc->sample_period)
183 {
184 enc->sample_count = 0;
185
186 // check for index event before the encoder reading
187 index_event_before = indextFlag;
188
189 // read the encoder
190 enc_val = posnCounts;
191
192 // compute the mechanical angle
193 // compute the mechanical angle
194 temp = (enc_val)*enc->mech_angle_gain;
195 // add in calibrated offset
196 temp += enc->enc_zero_offset;
197 // convert to electrical angle
198 temp = temp * enc->num_pole_pairs;
199 // wrap around 1.0
200 temp &= ((uint32_t) 0x00ffffff);
201
202 enc->enc_elec_angle = (_iq)temp;
203
204 /*********************/
205 /* compute the SPEED */
206 /*********************/
207
208 // read QEP direction from quadrature direction latch flag
209 dir = dirFlag;
210
211 // check for index event after the encoder reading
212 index_event_after = indextFlag;
213
214 // handle a rollover event
215 if (index_event_after)
216 {
217 if (dir)
218 {
219 delta_enc = enc_val + (4*enc->num_enc_slots-1) - enc->prev_enc;
220 }
221 else
222 {
223 delta_enc = enc->prev_enc + (4*enc->num_enc_slots-1) - enc_val;
224 }
225 }
226 else
227 {
228 delta_enc = enc_val - enc->prev_enc;
229 }
230
231 // save off the delta encoder value in the data structure only if the index event before and after are the same
232 if ((index_event_after == index_event_before) && (abs(delta_enc) < enc->num_enc_slots))
233 enc->delta_enc = delta_enc;
234
235 // log the startup data
236 switch(enc->log_state)
237 {
238 // wait for run flag to be set
239 case ENC_LOG_STATE_IDLE:
240 {
241 if (enc->run_flag)
242 {
243 enc->run_flag = 0;
244 enc->log_idx = 0;
245 enc->log_state = ENC_LOG_STATE_FREERUN;
246 }
247 }
248 break;
249
250 // collect data round robin until there's a trigger event
251 case ENC_LOG_STATE_FREERUN:
252 {
253 enc->log[enc->log_idx] = (int16_t)enc_val;
254 enc->log_idx = enc->log_idx + 1;
255 if (enc->log_idx > ENC_LOG_LEN)
256 enc->log_idx = 0;
257 if (abs(delta_enc)>ENC_LOG_DELTA_TRIGGER_THRES)
258 {
259 enc->trigger_delta = delta_enc;
260 enc->trigger_idx = enc->log_idx;
261 enc->post_trigger_cnt = enc->post_trigger_len;
262 enc->log_state = ENC_LOG_STATE_ACQUIRE;
263 }
264 }
265 break;
266
267 // when trigger occurs collect 1/2 a buffer of post-trigger information
268 case ENC_LOG_STATE_ACQUIRE:
269 {
270 enc->log[enc->log_idx] = (int16_t)enc_val;
271 enc->log_idx = enc->log_idx + 1;
272 if (enc->log_idx > ENC_LOG_LEN)
273 enc->log_idx = 0;
274 enc->post_trigger_cnt = enc->post_trigger_cnt - 1;
275 if (enc->post_trigger_cnt == 0)
276 enc->log_state = ENC_LOG_STATE_IDLE;
277 }
278 break;
279 }
280
281 // shift it to Q24
282 temp = enc->delta_enc*enc->speed_gain;
283
284 // LPF the encoder
285 temp = (enc->speed_lpf_cy)*enc->speed_lpf_out + (enc->speed_lpf_cx)*temp;
286 temp >>= ENC_SPEED_COEFF_Q;
287 enc->speed_lpf_out = temp;
288
289 // copy the current into the previous value
290 enc->prev_enc = enc_val;
291 }
292 else
293 {
294 enc->sample_count = sample_count;
295 }
296
297 } // end of ENC_run() function
298
299
300 int16_t ENC_getSpeedRPM(ENC_Handle encHandle)
301 {
302 ENC_Obj *enc;
303 _iq temp;
304
305 // create an object pointer for manipulation
306 enc = (ENC_Obj *) encHandle;
307
308 temp = (enc->speed_lpf_out >> ENC_RPM_Q1);
309 temp = (temp * enc->rpm_gain) >> ENC_RPM_Q2;
310
311 return (int16_t) temp;
312 } // end of ENC_getSpeedRPM() function
313
314
315 // end of file
_ENC_Obj_::post_trigger_len
int16_t post_trigger_len
encoder log post trigger length
Definition: enc.h:109
_ENC_Obj_::trigger_idx
int16_t trigger_idx
index where trigger event happened
Definition: enc.h:111
ENC_RPM_Q1
#define ENC_RPM_Q1
Definition: enc.h:67
_ENC_Obj_::run_flag
int16_t run_flag
encoder log free run flag
Definition: enc.h:108
ENC_run
void ENC_run(ENC_Handle encHandle, uint32_t posnCounts, uint16_t indextFlag, uint16_t dirFlag, int16_t log_flag)
Based on the encoder reading, computes the electrical angle and the electrical "speed".
Definition: enc.c:163
_ENC_Obj_::full_scale_freq
float_t full_scale_freq
full scale frequency
Definition: enc.h:100
ENC_LOG_STATE_ACQUIRE
acquire state
Definition: enc.h:80
_ENC_Obj_::speed_lpf_cy
int32_t speed_lpf_cy
speed output coefficient
Definition: enc.h:105
_ENC_Obj_::delta_enc
int32_t delta_enc
encoder count delta
Definition: enc.h:99
ENC_2PI
#define ENC_2PI
Definition: enc.h:65
_ENC_Obj_::sample_count
int16_t sample_count
when it reaches the sample period, collect & process encoder data
Definition: enc.h:88
ENC_LOG_DELTA_TRIGGER_THRES
#define ENC_LOG_DELTA_TRIGGER_THRES
Definition: enc.h:69
ENC_SPEED_COEFF_Q
#define ENC_SPEED_COEFF_Q
Definition: enc.h:63
ENC_LOG_LEN
#define ENC_LOG_LEN
Definition: enc.h:66
_ENC_Obj_::speed_lpf_cx
int32_t speed_lpf_cx
speed input coefficient
Definition: enc.h:104
ENC_RPM_Q2
#define ENC_RPM_Q2
Definition: enc.h:68
_ENC_Obj_::rpm_gain
int32_t rpm_gain
gain which converts the Q24 normalized electrical freq to RPM
Definition: enc.h:102
_iq
long _iq
Definition: 32b/IQmathLib.h:223
_ENC_Obj_::num_pole_pairs
uint16_t num_pole_pairs
number of pole pairs in motor
Definition: enc.h:90
_ENC_Obj_::prev_enc
int32_t prev_enc
previous encoder reading
Definition: enc.h:98
ENC_LOG_STATE_FREERUN
freerun state
Definition: enc.h:79
ENC_SPEED_COEFF_SCALING
#define ENC_SPEED_COEFF_SCALING
Definition: enc.h:64
_ENC_Obj_::enc_zero_offset
uint32_t enc_zero_offset
encoder zero offset in counts
Definition: enc.h:93
_ENC_Obj_
Defines the encoder object.
Definition: enc.h:86
_ENC_Obj_::sample_period
int16_t sample_period
sample period of encoder processing
Definition: enc.h:89
_ENC_Obj_::trigger_delta
int16_t trigger_delta
calculated delta when trigger happened
Definition: enc.h:112
_ENC_Obj_::incremental_slip
int32_t incremental_slip
incremental amount of slip
Definition: enc.h:95
ENC_init
ENC_Handle ENC_init(void *pMemory, const size_t numBytes)
Initializes the encoder object.
Definition: enc.c:55
ENC_calcElecAngle
void ENC_calcElecAngle(ENC_Handle encHandle, uint32_t posnCounts)
Reads encoder and returns the electrical degrees in Q24 format.
Definition: enc.c:129
ENC_setup
void ENC_setup(ENC_Handle encHandle, const int16_t sample_period, const uint16_t num_pole_pairs, const uint16_t num_enc_slots, const uint32_t enc_zero_offset, const float_t full_scale_freq, const float_t speed_update_freq, const float_t speed_cutoff)
Initializes encoder object parameters.
Definition: enc.c:68
_ENC_Obj_::enc_elec_angle
int32_t enc_elec_angle
encoder current electrical angle
Definition: enc.h:94
_ENC_Obj_::log_idx
int32_t log_idx
encoder log index
Definition: enc.h:113
_ENC_Obj_::post_trigger_cnt
int16_t post_trigger_cnt
encoder log post trigger counter
Definition: enc.h:110
_ENC_Obj_::num_enc_slots
uint16_t num_enc_slots
number of encoder slots
Definition: enc.h:91
enc.h
Contains the public interface to the encoder module routines.
_ENC_Obj_::enc_magnetic_angle
int32_t enc_magnetic_angle
encoder current magnetic angle (compensated for slip)
Definition: enc.h:97
_ENC_Obj_::enc_slip_angle
int32_t enc_slip_angle
amount of total slip
Definition: enc.h:96
_ENC_Obj_::log_state
ENC_LOG_State_e log_state
encoder log state
Definition: enc.h:107
_ENC_Obj_::speed_lpf_out
int32_t speed_lpf_out
speed lpf output
Definition: enc.h:106
ENC_getSpeedRPM
int16_t ENC_getSpeedRPM(ENC_Handle encHandle)
Returns the filtered speed in RPM.
Definition: enc.c:300
_ENC_Obj_::speed_gain
int32_t speed_gain
gain which converts a difference in encoder counts to Q24 normalized electrical freq ...
Definition: enc.h:101
_ENC_Obj_::speed_cutoff
float_t speed_cutoff
speed cutoff frequency in Hz
Definition: enc.h:103
ENC_LOG_STATE_IDLE
idle state
Definition: enc.h:78
ENC_Handle
struct _ENC_Obj_ * ENC_Handle
Defines the encoder handle.
Definition: enc.h:120
_ENC_Obj_::mech_angle_gain
_iq mech_angle_gain
gain which converts the encoder counts to Q24 mechanical degrees
Definition: enc.h:92
ENC_SPEED_SCALING_FACTOR
#define ENC_SPEED_SCALING_FACTOR
Definition: enc.h:60
_ENC_Obj_::log
int16_t log[ENC_LOG_LEN]
encoder log length
Definition: enc.h:114
float_t
float float_t
Defines the portable data type for 32 bit, signed floating-point data.
Definition: types.h:121