mirror of https://github.com/rusefi/bldc.git
Nunchuk implementation (Nyko Kama tested), mah and wh counting
This commit is contained in:
parent
ce3d1f9596
commit
81a6d84113
|
@ -25,6 +25,7 @@
|
|||
#include "app.h"
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "hw.h"
|
||||
|
||||
// Private variables
|
||||
static app_configuration appconf;
|
||||
|
@ -41,11 +42,13 @@ void app_init(app_configuration *conf) {
|
|||
break;
|
||||
|
||||
case APP_UART:
|
||||
hw_stop_i2c_uart();
|
||||
app_uartcomm_configure(appconf.app_uart_baudrate);
|
||||
app_uartcomm_start();
|
||||
break;
|
||||
|
||||
case APP_PPM_UART:
|
||||
hw_stop_i2c_uart();
|
||||
app_ppm_configure(appconf.app_ppm_ctrl_type, appconf.app_ppm_pid_max_erpm,
|
||||
appconf.app_ppm_hyst, appconf.app_ppm_pulse_start, appconf.app_ppm_pulse_width,
|
||||
appconf.app_ppm_rpm_lim_start, appconf.app_ppm_rpm_lim_end);
|
||||
|
@ -54,8 +57,15 @@ void app_init(app_configuration *conf) {
|
|||
app_uartcomm_start();
|
||||
break;
|
||||
|
||||
case APP_NUNCHUK:
|
||||
app_nunchuk_configure(appconf.app_chuk_ctrl_type, appconf.app_chuk_hyst,
|
||||
appconf.app_chuk_rpm_lim_start, appconf.app_chuk_rpm_lim_end);
|
||||
app_nunchuk_start();
|
||||
break;
|
||||
|
||||
case APP_CUSTOM:
|
||||
#ifdef USE_APP_STEN
|
||||
hw_stop_i2c_uart();
|
||||
app_sten_init();
|
||||
#endif
|
||||
#ifdef USE_APP_GURGALOF
|
||||
|
@ -85,4 +95,6 @@ void app_set_configuration(app_configuration *conf) {
|
|||
appconf.app_ppm_hyst, appconf.app_ppm_pulse_start, appconf.app_ppm_pulse_width,
|
||||
appconf.app_ppm_rpm_lim_start, appconf.app_ppm_rpm_lim_end);
|
||||
app_uartcomm_configure(appconf.app_uart_baudrate);
|
||||
app_nunchuk_configure(appconf.app_chuk_ctrl_type, appconf.app_chuk_hyst,
|
||||
appconf.app_chuk_rpm_lim_start, appconf.app_chuk_rpm_lim_end);
|
||||
}
|
||||
|
|
|
@ -38,6 +38,10 @@ void app_ppm_configure(ppm_control_type ctrlt, float pme, float hyst,
|
|||
float pulse_start, float pulse_width, float lim_rpm_start, float lim_rpm_end);
|
||||
void app_uartcomm_start(void);
|
||||
void app_uartcomm_configure(uint32_t baudrate);
|
||||
void app_nunchuk_start(void);
|
||||
void app_nunchuk_configure(chuk_control_type ctrlt,
|
||||
float hyst, float lim_rpm_start, float lim_rpm_end);
|
||||
float app_nunchuk_get_decoded_chuk(void);
|
||||
|
||||
// Custom apps
|
||||
void app_gurgalof_init(void);
|
||||
|
|
|
@ -0,0 +1,252 @@
|
|||
/*
|
||||
* app_nunchuk.c
|
||||
*
|
||||
* Created on: 18 okt 2014
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#include "app.h"
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "hw.h"
|
||||
#include "mcpwm.h"
|
||||
#include "commands.h"
|
||||
#include "utils.h"
|
||||
#include "timeout.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
// Types
|
||||
typedef struct {
|
||||
int js_x;
|
||||
int js_y;
|
||||
int acc_x;
|
||||
int acc_y;
|
||||
int acc_z;
|
||||
bool bt_c;
|
||||
bool bt_z;
|
||||
} CHUCK_DATA;
|
||||
|
||||
// Threads
|
||||
static msg_t chuk_thread(void *arg);
|
||||
static WORKING_AREA(chuk_thread_wa, 1024);
|
||||
static msg_t output_thread(void *arg);
|
||||
static WORKING_AREA(output_thread_wa, 1024);
|
||||
|
||||
// Private variables
|
||||
static volatile chuk_control_type ctrl_type = CHUK_CTRL_TYPE_CURRENT_NOREV;
|
||||
static volatile bool is_running = false;
|
||||
static volatile float hysteres = 0.15;
|
||||
static volatile float rpm_lim_start = 200000.0;
|
||||
static volatile float rpm_lim_end = 250000.0;
|
||||
static volatile CHUCK_DATA chuck_data;
|
||||
static i2cflags_t i2c_errors = 0;
|
||||
static int chuck_error = 0;
|
||||
|
||||
// I2C configuration
|
||||
static const I2CConfig i2cfg = {
|
||||
OPMODE_I2C,
|
||||
100000,
|
||||
FAST_DUTY_CYCLE_2
|
||||
};
|
||||
|
||||
void app_nunchuk_configure(chuk_control_type ctrlt,
|
||||
float hyst, float lim_rpm_start, float lim_rpm_end) {
|
||||
ctrl_type = ctrlt;
|
||||
hysteres = hyst;
|
||||
rpm_lim_start = lim_rpm_start;
|
||||
rpm_lim_end = lim_rpm_end;
|
||||
}
|
||||
|
||||
void app_nunchuk_start(void) {
|
||||
chuck_data.js_y = 128;
|
||||
|
||||
chThdCreateStatic(chuk_thread_wa, sizeof(chuk_thread_wa), NORMALPRIO, chuk_thread, NULL);
|
||||
chThdCreateStatic(output_thread_wa, sizeof(output_thread_wa), NORMALPRIO, output_thread, NULL);
|
||||
}
|
||||
|
||||
float app_nunchuk_get_decoded_chuk(void) {
|
||||
return ((float)chuck_data.js_y - 128.0) / 128.0;
|
||||
}
|
||||
|
||||
static msg_t chuk_thread(void *arg) {
|
||||
(void)arg;
|
||||
|
||||
chRegSetThreadName("APP Nunchuk");
|
||||
is_running = true;
|
||||
|
||||
uint8_t rxbuf[10];
|
||||
uint8_t txbuf[10];
|
||||
msg_t status = RDY_OK;
|
||||
systime_t tmo = MS2ST(5);
|
||||
i2caddr_t chuck_addr = 0x52;
|
||||
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
i2cStart(&HW_I2C_DEV, &i2cfg);
|
||||
|
||||
chThdSleepMilliseconds(10);
|
||||
|
||||
for(;;) {
|
||||
txbuf[0] = 0xF0;
|
||||
txbuf[1] = 0x55;
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 2, rxbuf, 0, tmo);
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
|
||||
txbuf[0] = 0xFB;
|
||||
txbuf[1] = 0x00;
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 2, rxbuf, 0, tmo);
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
|
||||
txbuf[0] = 0x00;
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 1, rxbuf, 0, tmo);
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
|
||||
chThdSleepMilliseconds(3);
|
||||
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
status = i2cMasterReceiveTimeout(&HW_I2C_DEV, chuck_addr, rxbuf, 6, tmo);
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
|
||||
if (status == RDY_OK){
|
||||
i2c_errors = 0;
|
||||
|
||||
static uint8_t last_buffer[10];
|
||||
static int same_cnt = 0;
|
||||
|
||||
int same = 1;
|
||||
for (int i = 0;i < 6;i++) {
|
||||
if (last_buffer[i] != rxbuf[i]) {
|
||||
same = 0;
|
||||
}
|
||||
}
|
||||
|
||||
memcpy(last_buffer, rxbuf, 6);
|
||||
|
||||
if (same) {
|
||||
same_cnt++;
|
||||
} else {
|
||||
same_cnt = 0;
|
||||
|
||||
chuck_data.js_x = rxbuf[0];
|
||||
chuck_data.js_y = rxbuf[1];
|
||||
chuck_data.acc_x = (rxbuf[2] << 2) | ((rxbuf[5] >> 2) & 3);
|
||||
chuck_data.acc_y = (rxbuf[3] << 2) | ((rxbuf[5] >> 4) & 3);
|
||||
chuck_data.acc_z = (rxbuf[4] << 2) | ((rxbuf[5] >> 6) & 3);
|
||||
chuck_data.bt_z = !((rxbuf[5] >> 0) & 1);
|
||||
chuck_data.bt_c = !((rxbuf[5] >> 1) & 1);
|
||||
}
|
||||
|
||||
if (same_cnt < 100) {
|
||||
chuck_error = 0;
|
||||
timeout_reset();
|
||||
} else {
|
||||
chuck_error = 1;
|
||||
// Do nothing and let the motor controller handle the problem
|
||||
}
|
||||
} else {
|
||||
i2c_errors = i2cGetErrors(&HW_I2C_DEV);
|
||||
chuck_error = 2;
|
||||
// Do nothing and let the motor controller handle the problem
|
||||
}
|
||||
|
||||
chThdSleepMilliseconds(10);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static msg_t output_thread(void *arg) {
|
||||
(void)arg;
|
||||
|
||||
chRegSetThreadName("Nunchuk output");
|
||||
|
||||
for(;;) {
|
||||
chThdSleepMilliseconds(1);
|
||||
|
||||
if (timeout_has_timeout() || chuck_error != 0 || ctrl_type == CHUK_CTRL_TYPE_NONE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float out_val = app_nunchuk_get_decoded_chuk();
|
||||
|
||||
out_val /= (1.0 - hysteres);
|
||||
|
||||
if (out_val > hysteres) {
|
||||
out_val -= hysteres;
|
||||
} else if (out_val < -hysteres) {
|
||||
out_val += hysteres;
|
||||
} else {
|
||||
out_val = 0.0;
|
||||
}
|
||||
|
||||
// If c is pressed and no throttle is used, maintain the current speed with PID control
|
||||
static bool was_pid = false;
|
||||
|
||||
if (chuck_data.bt_c && out_val == 0.0) {
|
||||
static float pid_rpm = 0.0;
|
||||
|
||||
if (!was_pid) {
|
||||
was_pid = true;
|
||||
pid_rpm = mcpwm_get_rpm();
|
||||
}
|
||||
|
||||
if (ctrl_type == CHUK_CTRL_TYPE_CURRENT || pid_rpm > 0.0) {
|
||||
mcpwm_set_pid_speed(pid_rpm);
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
was_pid = false;
|
||||
|
||||
float current = 0;
|
||||
bool current_mode_brake = false;
|
||||
const volatile mc_configuration *mcconf = mcpwm_get_configuration();
|
||||
|
||||
if (out_val >= 0.0) {
|
||||
current = out_val * mcconf->l_current_max;
|
||||
} else {
|
||||
current = out_val * fabsf(mcconf->l_current_min);
|
||||
current_mode_brake = ctrl_type == CHUK_CTRL_TYPE_CURRENT_NOREV;
|
||||
}
|
||||
|
||||
if (current_mode_brake) {
|
||||
mcpwm_set_brake_current(current);
|
||||
} else {
|
||||
|
||||
// Apply soft RPM limit if z is not pressed.
|
||||
if (!chuck_data.bt_z) {
|
||||
float rpm = mcpwm_get_rpm();
|
||||
if (rpm > rpm_lim_end && current > 0.0) {
|
||||
current = mcconf->cc_min_current;
|
||||
} else if (rpm > rpm_lim_start && current > 0.0) {
|
||||
current = utils_map(rpm, rpm_lim_start, rpm_lim_end, current, mcconf->cc_min_current);
|
||||
} else if (rpm < -rpm_lim_end && current < 0.0) {
|
||||
current = mcconf->cc_min_current;
|
||||
} else if (rpm < -rpm_lim_start && current < 0.0) {
|
||||
rpm = -rpm;
|
||||
current = -current;
|
||||
current = utils_map(rpm, rpm_lim_start, rpm_lim_end, current, mcconf->cc_min_current);
|
||||
current = -current;
|
||||
}
|
||||
}
|
||||
|
||||
mcpwm_set_current(current);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
|
@ -2,6 +2,7 @@ APPSRC = applications/app.c \
|
|||
applications/app_ppm.c \
|
||||
applications/app_sten.c \
|
||||
applications/app_gurgalof.c \
|
||||
applications/app_uartcomm.c
|
||||
applications/app_uartcomm.c \
|
||||
applications/app_nunchuk.c
|
||||
|
||||
APPINC = applications
|
||||
|
|
19
commands.c
19
commands.c
|
@ -265,6 +265,7 @@ void commands_process_packet(unsigned char *data, unsigned char len) {
|
|||
appconf.timeout_msec = buffer_get_uint32(data, &ind);
|
||||
appconf.timeout_brake_current = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_to_use = data[ind++];
|
||||
|
||||
appconf.app_ppm_ctrl_type = data[ind++];
|
||||
appconf.app_ppm_pid_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_ppm_hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
|
@ -275,6 +276,11 @@ void commands_process_packet(unsigned char *data, unsigned char len) {
|
|||
|
||||
appconf.app_uart_baudrate = buffer_get_uint32(data, &ind);
|
||||
|
||||
appconf.app_chuk_ctrl_type = data[ind++];
|
||||
appconf.app_chuk_hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_chuk_rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_chuk_rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
|
||||
conf_general_store_app_configuration(&appconf);
|
||||
app_set_configuration(&appconf);
|
||||
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
|
||||
|
@ -288,6 +294,7 @@ void commands_process_packet(unsigned char *data, unsigned char len) {
|
|||
buffer_append_uint32(send_buffer, appconf.timeout_msec, &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.timeout_brake_current * 1000.0), &ind);
|
||||
send_buffer[ind++] = appconf.app_to_use;
|
||||
|
||||
send_buffer[ind++] = appconf.app_ppm_ctrl_type;
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pid_max_erpm * 1000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_hyst * 1000.0), &ind);
|
||||
|
@ -298,6 +305,11 @@ void commands_process_packet(unsigned char *data, unsigned char len) {
|
|||
|
||||
buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
|
||||
|
||||
send_buffer[ind++] = appconf.app_chuk_ctrl_type;
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_hyst * 1000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_rpm_lim_start * 1000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_rpm_lim_end * 1000.0), &ind);
|
||||
|
||||
send_packet(send_buffer, ind);
|
||||
break;
|
||||
|
||||
|
@ -340,6 +352,13 @@ void commands_process_packet(unsigned char *data, unsigned char len) {
|
|||
send_packet(send_buffer, ind);
|
||||
break;
|
||||
|
||||
case COMM_GET_DECODED_CHUK:
|
||||
ind = 0;
|
||||
send_buffer[ind++] = COMM_GET_DECODED_CHUK;
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_nunchuk_get_decoded_chuk() * 1000000.0), &ind);
|
||||
send_packet(send_buffer, ind);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -136,6 +136,7 @@ void conf_general_read_app_configuration(app_configuration *conf) {
|
|||
conf->timeout_msec = 1000;
|
||||
conf->timeout_brake_current = 0.0;
|
||||
conf->app_to_use = APP_NONE;
|
||||
|
||||
conf->app_ppm_ctrl_type = PPM_CTRL_TYPE_CURRENT;
|
||||
conf->app_ppm_pid_max_erpm = 15000;
|
||||
conf->app_ppm_hyst = 0.15;
|
||||
|
@ -143,7 +144,13 @@ void conf_general_read_app_configuration(app_configuration *conf) {
|
|||
conf->app_ppm_pulse_width = 1.0;
|
||||
conf->app_ppm_rpm_lim_start = 200000.0;
|
||||
conf->app_ppm_rpm_lim_end = 250000.0;
|
||||
|
||||
conf->app_uart_baudrate = 115200;
|
||||
|
||||
conf->app_chuk_ctrl_type = CHUK_CTRL_TYPE_CURRENT_NOREV;
|
||||
conf->app_chuk_hyst = 0.15;
|
||||
conf->app_chuk_rpm_lim_start = 20000.0;
|
||||
conf->app_chuk_rpm_lim_end = 25000.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
17
datatypes.h
17
datatypes.h
|
@ -121,6 +121,7 @@ typedef enum {
|
|||
APP_PPM,
|
||||
APP_UART,
|
||||
APP_PPM_UART,
|
||||
APP_NUNCHUK,
|
||||
APP_CUSTOM
|
||||
} app_use;
|
||||
|
||||
|
@ -136,6 +137,13 @@ typedef enum {
|
|||
PPM_CTRL_TYPE_PID_NOREV
|
||||
} ppm_control_type;
|
||||
|
||||
// Nunchuk control types
|
||||
typedef enum {
|
||||
CHUK_CTRL_TYPE_NONE = 0,
|
||||
CHUK_CTRL_TYPE_CURRENT,
|
||||
CHUK_CTRL_TYPE_CURRENT_NOREV
|
||||
} chuk_control_type;
|
||||
|
||||
typedef struct {
|
||||
// Settings
|
||||
uint32_t timeout_msec;
|
||||
|
@ -155,6 +163,12 @@ typedef struct {
|
|||
|
||||
// UART application settings
|
||||
uint32_t app_uart_baudrate;
|
||||
|
||||
// Nunchuk
|
||||
chuk_control_type app_chuk_ctrl_type;
|
||||
float app_chuk_hyst;
|
||||
float app_chuk_rpm_lim_start;
|
||||
float app_chuk_rpm_lim_end;
|
||||
} app_configuration;
|
||||
|
||||
// Communication commands
|
||||
|
@ -178,7 +192,8 @@ typedef enum {
|
|||
COMM_DETECT_MOTOR_PARAM,
|
||||
COMM_REBOOT,
|
||||
COMM_ALIVE,
|
||||
COMM_GET_DECODED_PPM
|
||||
COMM_GET_DECODED_PPM,
|
||||
COMM_GET_DECODED_CHUK
|
||||
} COMM_PACKET_ID;
|
||||
|
||||
#endif /* DATATYPES_H_ */
|
||||
|
|
|
@ -87,7 +87,7 @@
|
|||
* @brief Enables the I2C subsystem.
|
||||
*/
|
||||
#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__)
|
||||
#define HAL_USE_I2C FALSE
|
||||
#define HAL_USE_I2C TRUE
|
||||
#endif
|
||||
|
||||
/**
|
||||
|
|
|
@ -41,5 +41,6 @@
|
|||
void hw_init_gpio(void);
|
||||
void hw_setup_adc_channels(void);
|
||||
void hw_setup_servo_outputs(void);
|
||||
void hw_stop_i2c_uart(void);
|
||||
|
||||
#endif /* HW_H_ */
|
||||
|
|
|
@ -143,4 +143,14 @@ void hw_setup_servo_outputs(void) {
|
|||
servos[1].pos = 0;
|
||||
}
|
||||
|
||||
void hw_stop_i2c_uart(void) {
|
||||
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
|
||||
|
||||
uartStop(&HW_UART_DEV);
|
||||
i2cStop(&HW_I2C_DEV);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -113,4 +113,12 @@
|
|||
#define HW_ICU_GPIO GPIOB
|
||||
#define HW_ICU_PIN 5
|
||||
|
||||
// I2C Peripheral
|
||||
#define HW_I2C_DEV I2CD2
|
||||
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
|
||||
#define HW_I2C_SCL_PORT GPIOB
|
||||
#define HW_I2C_SCL_PIN 10
|
||||
#define HW_I2C_SDA_PORT GPIOB
|
||||
#define HW_I2C_SDA_PIN 11
|
||||
|
||||
#endif /* HW_40_H_ */
|
||||
|
|
|
@ -143,4 +143,14 @@ void hw_setup_servo_outputs(void) {
|
|||
servos[1].pos = 0;
|
||||
}
|
||||
|
||||
void hw_stop_i2c_uart(void) {
|
||||
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
|
||||
|
||||
uartStop(&HW_UART_DEV);
|
||||
i2cStop(&HW_I2C_DEV);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -113,4 +113,12 @@
|
|||
#define HW_ICU_GPIO GPIOB
|
||||
#define HW_ICU_PIN 5
|
||||
|
||||
// I2C Peripheral
|
||||
#define HW_I2C_DEV I2CD2
|
||||
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
|
||||
#define HW_I2C_SCL_PORT GPIOB
|
||||
#define HW_I2C_SCL_PIN 10
|
||||
#define HW_I2C_SDA_PORT GPIOB
|
||||
#define HW_I2C_SDA_PIN 11
|
||||
|
||||
#endif /* HW_40_H_ */
|
||||
|
|
|
@ -148,4 +148,14 @@ void hw_setup_servo_outputs(void) {
|
|||
servos[1].pos = 0;
|
||||
}
|
||||
|
||||
void hw_stop_i2c_uart(void) {
|
||||
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
|
||||
|
||||
uartStop(&HW_UART_DEV);
|
||||
i2cStop(&HW_I2C_DEV);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -123,4 +123,12 @@
|
|||
#define HW_ICU_GPIO GPIOB
|
||||
#define HW_ICU_PIN 5
|
||||
|
||||
// I2C Peripheral
|
||||
#define HW_I2C_DEV I2CD2
|
||||
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
|
||||
#define HW_I2C_SCL_PORT GPIOB
|
||||
#define HW_I2C_SCL_PIN 10
|
||||
#define HW_I2C_SDA_PORT GPIOB
|
||||
#define HW_I2C_SDA_PIN 11
|
||||
|
||||
#endif /* HW_R2_H_ */
|
||||
|
|
31
mcpwm.c
31
mcpwm.c
|
@ -1069,14 +1069,14 @@ static void run_pid_controller(void) {
|
|||
|
||||
// PID is off. Return.
|
||||
if (control_mode != CONTROL_MODE_SPEED) {
|
||||
i_term = 0;
|
||||
i_term = dutycycle_now;
|
||||
prev_error = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// Too low RPM set. Stop and return.
|
||||
if (fabsf(speed_pid_set_rpm) < conf.s_pid_min_rpm) {
|
||||
i_term = 0;
|
||||
i_term = dutycycle_now;
|
||||
prev_error = 0;
|
||||
mcpwm_set_duty(0.0);
|
||||
return;
|
||||
|
@ -1633,14 +1633,25 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
motor_current_iterations++;
|
||||
input_current_iterations++;
|
||||
|
||||
if (fabsf(current) > 0.5) {
|
||||
float curr_diff = current_in * 1.0 / switching_frequency_now;
|
||||
if (curr_diff > 0.0) {
|
||||
amp_seconds += curr_diff;
|
||||
watt_seconds += curr_diff * input_voltage;
|
||||
} else {
|
||||
amp_seconds_charged -= curr_diff;
|
||||
watt_seconds_charged -= curr_diff * input_voltage;
|
||||
if (fabsf(current) > 1.0) {
|
||||
// Some extra filtering
|
||||
static float curr_diff_sum = 0.0;
|
||||
static float curr_diff_samples = 0;
|
||||
|
||||
curr_diff_sum += current_in / switching_frequency_now;
|
||||
curr_diff_samples += 1.0 / switching_frequency_now;
|
||||
|
||||
if (curr_diff_samples >= 0.01) {
|
||||
if (curr_diff_sum > 0.0) {
|
||||
amp_seconds += curr_diff_sum;
|
||||
watt_seconds += curr_diff_sum * input_voltage;
|
||||
} else {
|
||||
amp_seconds_charged -= curr_diff_sum;
|
||||
watt_seconds_charged -= curr_diff_sum * input_voltage;
|
||||
}
|
||||
|
||||
curr_diff_samples = 0.0;
|
||||
curr_diff_sum = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -129,7 +129,7 @@
|
|||
* I2C driver system settings.
|
||||
*/
|
||||
#define STM32_I2C_USE_I2C1 FALSE
|
||||
#define STM32_I2C_USE_I2C2 FALSE
|
||||
#define STM32_I2C_USE_I2C2 TRUE
|
||||
#define STM32_I2C_USE_I2C3 FALSE
|
||||
#define STM32_I2C_I2C1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
|
||||
#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
|
||||
|
|
Loading…
Reference in New Issue