rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Nunchuk implementation (Nyko Kama tested), mah and wh counting

This commit is contained in:
Benjamin Vedder 2014-10-19 01:00:53 +02:00
parent ce3d1f9596
commit 81a6d84113
17 changed files with 390 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
applications/app.c
View File

@ -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);
}

4
applications/app.h
View File

@ -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);

252
applications/app_nunchuk.c Normal file
View File

@ -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;
}

3
applications/applications.mk
View File

@ -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
View File

@ -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;
}

7
conf_general.c
View File

@ -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
View File

@ -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_ */

2
halconf.h
View File

@ -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
/**

1
hwconf/hw.h
View File

@ -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_ */

10
hwconf/hw_40.c
View File

@ -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

8
hwconf/hw_40.h
View File

@ -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_ */

10
hwconf/hw_bw.c
View File

@ -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

8
hwconf/hw_bw.h
View File

@ -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_ */

10
hwconf/hw_r2.c
View File

@ -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

8
hwconf/hw_r2.h
View File

@ -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_ */

29
mcpwm.c
View File

@ -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;
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;
watt_seconds_charged -= curr_diff * input_voltage;
amp_seconds_charged -= curr_diff_sum;
watt_seconds_charged -= curr_diff_sum * input_voltage;
}
curr_diff_samples = 0.0;
curr_diff_sum = 0.0;
}
}

2
mcuconf.h
View File

@ -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)