forked from stancecoke/BMSBattery_S_controllers_firmware
/
cruise_control.c
executable file
·123 lines (108 loc) · 2.55 KB
/
cruise_control.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
/*
* Released under the GPL License, Version 3
*/
#include <stdint.h>
#include <stdio.h>
#include "stm8s.h"
#include "ACAcommons.h"
#include "ACAcontrollerState.h"
#include "cruise_control.h"
#include "brake.h"
#include "gpio.h"
#include "motor.h"
#define USE_CRUISE_REVERSE
static uint16_t cruise_control_speed = 0;
void cruise_control_init(void) {
; // we used to have something to init ...
}
static int16_t ccthr_i;
uint8_t cruise_control_enabled(void) {
return !!cruise_control_speed;
}
uint16_t cruise_control_throttle(uint16_t erps) {
/* CC Off? Quick exit. */
if (!cruise_control_speed) {
ccthr_i = 0;
return 0;
}
int16_t erp_delta = cruise_control_speed - erps;
// bits: 2 sign, overflow; 10 output, 4 internal fraction.
int16_t ccthr = erp_delta * 5; // 0.25 (p, and then pi)
ccthr_i += (erp_delta * 3); // 0.125
if (ccthr_i > 0x3FC0) {
//printf("ih\r\n");
ccthr_i = 0x3FC0;
}
if (ccthr_i < 0) {
// printf("iL\r\n");
ccthr_i = 0;
}
ccthr += ccthr_i;
if (ccthr < 0) {
// printf("rL\r\n");
ccthr = 0;
}
if (ccthr > 0x3FC0) {
//printf("rH\r\n");
ccthr = 0x3FC0;
}
return ccthr >> 4;
}
void enable_cruise_control(uint16_t erps)
{
cruise_control_speed = erps;
if (!erps) {
ccthr_i = 0;
return;
}
if (ui16_BatteryCurrent < ui16_current_cal_b) {
ccthr_i = 0;
} else {
ccthr_i = ((ui16_BatteryCurrent - ui16_current_cal_b) * 0x3FC0UL) / ui16_battery_current_max_value;
}
}
void stop_cruise_control (void)
{
cruise_control_speed = 0;
ccthr_i = 0;
}
static uint8_t cruise_btn_state = 0;
static uint8_t cruise_btn_ctr = 0;
static uint8_t brake_state = 0;
void cruise_control_update(void)
{
uint8_t cruise_pressed = 0;
uint8_t cruise_button = !GPIO_ReadInputPin(CRUISE_PORT, CRUISE_PIN);
if ((!cruise_btn_state) && (cruise_button)) {
cruise_pressed = 1;
cruise_btn_state = 1;
cruise_btn_ctr++;
}
if ((cruise_btn_state) && (!cruise_button)) {
cruise_btn_state = 0;
}
if (brake_is_set()) {
stop_cruise_control();
if (!brake_state) cruise_btn_ctr = 0;
brake_state = 1;
#ifdef USE_CRUISE_REVERSE
if ((0 == ui16_virtual_erps_speed)&&(motor_direction_reverse)) {
motor_direction_reverse = 0;
}
#endif
} else {
#ifdef USE_CRUISE_REVERSE
if ( (0 == ui16_virtual_erps_speed) && (brake_state) &&
(!cruise_btn_state) && (cruise_btn_ctr == 2) &&
(ui16_momentary_throttle) ) {
motor_direction_reverse = 1;
}
// dont enable cruise while reversing...
if (motor_direction_reverse) {
cruise_pressed = 0;
}
#endif
brake_state = 0;
if (cruise_pressed) enable_cruise_control(ui16_virtual_erps_speed);
}
}