forked from vedderb/bldc
-
Notifications
You must be signed in to change notification settings - Fork 1
/
servo_dec.c
138 lines (119 loc) · 3.15 KB
/
servo_dec.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
/*
Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* servo_dec.c
*
* Created on: 20 jan 2013
* Author: benjamin
*/
#include "servo_dec.h"
#include "stm32f4xx_conf.h"
#include "ch.h"
#include "hal.h"
#include "hw.h"
/*
* Settings
*/
#define SERVO_NUM 1
#define TIMER_FREQ 1000000
// Private variables
static volatile systime_t last_update_time;
static volatile float servo_pos[SERVO_NUM];
static volatile float pulse_start = 1.0;
static volatile float pulse_width = 1.0;
// Function pointers
static void(*done_func)(void) = 0;
static void icuwidthcb(ICUDriver *icup) {
float len = ((float)icuGetWidth(icup) / ((float)TIMER_FREQ / 1000.0)) - pulse_start;
if (len > pulse_width) {
if (len < pulse_width * 1.2) {
len = pulse_width;
} else {
// Too long pulse. Most likely something is wrong.
len = -1.0;
}
}
if (len > 0.0) {
servo_pos[0] = (len * 2.0 - pulse_width) / pulse_width;
last_update_time = chTimeNow();
if (done_func) {
done_func();
}
}
}
static void icuperiodcb(ICUDriver *icup) {
(void)icup;
}
static ICUConfig icucfg = {
ICU_INPUT_ACTIVE_HIGH,
TIMER_FREQ,
icuwidthcb,
icuperiodcb,
NULL,
HW_ICU_CHANNEL,
0
};
/**
* Initialize the serve decoding driver.
*
* @param d_func
* A function that should be called every time the servo signals have been
* decoded. Can be NULL.
*/
void servodec_init(void (*d_func)(void)) {
icuStart(&ICUD3, &icucfg);
palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_ALTERNATE(HW_ICU_GPIO_AF));
icuEnable(&ICUD3);
// Set our function pointer
done_func = d_func;
}
/**
* Change the limits of how the servo pulses should be decoded.
*
* @param start
* The amount of milliseconds the pulse starts at (default is 1.0)
*
* @param width
* The width of the pulse in milliseconds (default is 1.0)
*/
void servodec_set_pulse_options(float start, float width) {
pulse_start = start;
pulse_width = width;
}
/**
* Get a decoded servo value.
*
* @param servo_num
* The servo index. If it is out of range, 0.0 will be returned.
*
* @return
* The servo value in the range [-1.0 1.0].
*/
float servodec_get_servo(int servo_num) {
if (servo_num < SERVO_NUM) {
return servo_pos[servo_num];
} else {
return 0.0;
}
}
/**
* Get the amount of milliseconds that has passed since
* the last time servo positions were received.
*
* @return
* The amount of milliseconds that have passed since an update.
*/
uint32_t servodec_get_time_since_update(void) {
return chTimeElapsedSince(last_update_time) / (CH_FREQUENCY / 1000);
}