mirror of https://github.com/rusefi/bldc.git
Add support for A200S V4.1, add alternative DC Cal, expose I2C for LSM6DS3
Adds support for A200S V4.1 Move A200S V4 to its own folder. Expose I2C for LSM6DS3 to allow other devices on the same I2C bus. (It is now setup the same as the other IMUs) Add alternative DC Calibration routine for Low side shunt hardware with very high mosfet output capacitance. The usual calibration routine causes an offset in the current calibration at high voltage from the capacitance discharging through the motor inductance when the single phase goes low. The alternative routine switches all phases at 50% to give a clean V0 state. This routine should not be run while the motor is spinning, so automatic calibration on startup is disabled when this routine is selected.
This commit is contained in:
Euan
parent
c4a43fd8d7
commit
31f238849c
|
|
@ -37,6 +37,7 @@ static volatile bool i2c_running = false;
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void terminal_cmd_doublepulse(int argc, const char** argv);
|
static void terminal_cmd_doublepulse(int argc, const char** argv);
|
||||||
|
void hw_a200s_setup_dac(void);
|
||||||
|
|
||||||
// I2C configuration
|
// I2C configuration
|
||||||
static const I2CConfig i2cfg = {
|
static const I2CConfig i2cfg = {
|
||||||
|
|
@ -70,7 +71,14 @@ void hw_init_gpio(void) {
|
||||||
palSetPadMode(GPIOC, 5,
|
palSetPadMode(GPIOC, 5,
|
||||||
PAL_MODE_OUTPUT_PUSHPULL |
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
PAL_STM32_OSPEED_HIGHEST);
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
DISABLE_GATE();
|
ENABLE_GATE();
|
||||||
|
// Lockout
|
||||||
|
palSetPadMode(GPIOB, 12, PAL_MODE_INPUT);
|
||||||
|
|
||||||
|
#ifdef HW_PROTECTION_CURR_TRIP
|
||||||
|
// DAC for trip current
|
||||||
|
hw_a200s_setup_dac();
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef HW_USE_BRK
|
#ifdef HW_USE_BRK
|
||||||
// BRK Fault pin
|
// BRK Fault pin
|
||||||
|
|
@ -79,6 +87,8 @@ void hw_init_gpio(void) {
|
||||||
// Soft Lockout
|
// Soft Lockout
|
||||||
palSetPadMode(BRK_GPIO, BRK_PIN, PAL_MODE_INPUT);
|
palSetPadMode(BRK_GPIO, BRK_PIN, PAL_MODE_INPUT);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// AUX
|
// AUX
|
||||||
AUX_OFF();
|
AUX_OFF();
|
||||||
|
|
@ -296,6 +306,33 @@ float hw_a200s_get_temp(void) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void hw_a200s_setup_dac(void) {
|
||||||
|
// GPIOA clock enable
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||||
|
|
||||||
|
// DAC Periph clock enable
|
||||||
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
|
||||||
|
|
||||||
|
// DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration
|
||||||
|
palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
|
||||||
|
//palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
|
// Enable both DAC channels with output buffer disabled to achieve rail-to-rail output
|
||||||
|
DAC->CR |= DAC_CR_EN1 | DAC_CR_BOFF1 | DAC_CR_EN2 | DAC_CR_BOFF2;
|
||||||
|
|
||||||
|
// Set hardware trip current
|
||||||
|
//hw_a200s_set_curr_trip(HW_PROTECTION_CURR_TRIP);
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_a200s_set_curr_trip(uint16_t current) {
|
||||||
|
// DAC is 12bit 4096
|
||||||
|
// each adc sample is 3.3 / ((0.0002/3) * 20) = 2475 / 4096 = 0.60424804687A
|
||||||
|
// So current / 0.6042 = trip value in ADC counts
|
||||||
|
// Then around the midpoint of 2048.
|
||||||
|
DAC->DHR12R1 = 2048 + ((float)current / 0.2417f); // High
|
||||||
|
DAC->DHR12R2 = 2048 - ((float)current / 0.2417f); // Low
|
||||||
|
}
|
||||||
|
|
||||||
void hw_a200s_reset_faults(void) {
|
void hw_a200s_reset_faults(void) {
|
||||||
palSetPad(HW_PROTECTION_CLEAR_GPIO, HW_PROTECTION_CLEAR_PIN);
|
palSetPad(HW_PROTECTION_CLEAR_GPIO, HW_PROTECTION_CLEAR_PIN);
|
||||||
chThdSleep(100);
|
chThdSleep(100);
|
||||||
|
|
@ -0,0 +1,28 @@
|
||||||
|
/*
|
||||||
|
Copyright 2018 Benjamin Vedder benjamin@vedder.se
|
||||||
|
|
||||||
|
This file is part of the VESC firmware.
|
||||||
|
|
||||||
|
The VESC firmware 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.
|
||||||
|
|
||||||
|
The VESC firmware 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef HW_A200S_V41_H_
|
||||||
|
#define HW_A200S_V41_H_
|
||||||
|
|
||||||
|
#define HW_A200S_V41
|
||||||
|
|
||||||
|
#include "hw_a200s_v41_core.h"
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* HW_A200S_V41_H_ */
|
||||||
|
|
@ -0,0 +1,562 @@
|
||||||
|
/*
|
||||||
|
Copyright 2018 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "hw.h"
|
||||||
|
#include "ch.h"
|
||||||
|
#include "hal.h"
|
||||||
|
#include "stm32f4xx_conf.h"
|
||||||
|
#include "utils.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include "mc_interface.h"
|
||||||
|
#include "commands.h"
|
||||||
|
#include "terminal.h"
|
||||||
|
#include "mcpwm.h"
|
||||||
|
#include "mcpwm_foc.h"
|
||||||
|
#include "gpdrive.h"
|
||||||
|
#include "app.h"
|
||||||
|
#include "mempools.h"
|
||||||
|
#include "timeout.h"
|
||||||
|
#include "stdio.h"
|
||||||
|
#include "imu.h"
|
||||||
|
|
||||||
|
// Variables
|
||||||
|
static volatile bool i2c_running = false;
|
||||||
|
static volatile bool drv_handshake_complete = false;
|
||||||
|
|
||||||
|
// Private functions
|
||||||
|
static void terminal_cmd_doublepulse(int argc, const char** argv);
|
||||||
|
static void hw_a200s_set_hardware_current_limits(void);
|
||||||
|
|
||||||
|
// I2C configuration
|
||||||
|
static const I2CConfig i2cfg = {
|
||||||
|
OPMODE_I2C,
|
||||||
|
100000,
|
||||||
|
STD_DUTY_CYCLE
|
||||||
|
};
|
||||||
|
|
||||||
|
void hw_init_gpio(void) {
|
||||||
|
// GPIO clock enable
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
|
||||||
|
|
||||||
|
// LEDs
|
||||||
|
palSetPadMode(GPIOB, 5,
|
||||||
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
palSetPadMode(GPIOB, 7,
|
||||||
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
|
||||||
|
// HW protection pins
|
||||||
|
// Disable
|
||||||
|
palSetPadMode(GPIOC, 5,
|
||||||
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
ENABLE_GATE();
|
||||||
|
// Lockout
|
||||||
|
palSetPadMode(GPIOB, 12, PAL_MODE_INPUT);
|
||||||
|
|
||||||
|
#ifdef HW_USE_BRK
|
||||||
|
// BRK Fault pin
|
||||||
|
palSetPadMode(BRK_GPIO, BRK_PIN, PAL_MODE_ALTERNATE(GPIO_AF_TIM1));
|
||||||
|
#else
|
||||||
|
// Soft Lockout
|
||||||
|
palSetPadMode(BRK_GPIO, BRK_PIN, PAL_MODE_INPUT);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
|
||||||
|
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
|
||||||
|
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_FLOATING);
|
||||||
|
|
||||||
|
// Hall sensors
|
||||||
|
palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
|
||||||
|
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
|
||||||
|
palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
|
||||||
|
|
||||||
|
// Phase filters
|
||||||
|
palSetPadMode(GPIOC, 9, PAL_MODE_OUTPUT_OPENDRAIN);
|
||||||
|
palSetPadMode(GPIOC, 13, PAL_MODE_OUTPUT_OPENDRAIN);
|
||||||
|
palSetPadMode(GPIOC, 14, PAL_MODE_OUTPUT_OPENDRAIN);
|
||||||
|
PHASE_FILTER_OFF();
|
||||||
|
|
||||||
|
// ADC Pins
|
||||||
|
palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
|
palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
|
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
|
|
||||||
|
//register terminal callbacks
|
||||||
|
terminal_register_command_callback(
|
||||||
|
"double_pulse",
|
||||||
|
"Start a double pulse test",
|
||||||
|
0,
|
||||||
|
terminal_cmd_doublepulse);
|
||||||
|
|
||||||
|
|
||||||
|
hw_a200s_reset_faults(); // Handshake with hardware protection
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_setup_adc_channels(void) {
|
||||||
|
|
||||||
|
// ADC1 regular channels
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles); // 0 - ADC_IND_CURR1
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 2, ADC_SampleTime_15Cycles); // 3 - ADC_IND_SENS1
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles); // 6 - ADC_IND_EXT
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 4, ADC_SampleTime_15Cycles); // 9 - ADC_IND_VREFINT
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 5, ADC_SampleTime_15Cycles); // 12 - ADC_IND_TEMP_MOS_2
|
||||||
|
|
||||||
|
// ADC2 regular channels
|
||||||
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles); // 1 - ADC_IND_CURR2
|
||||||
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 2, ADC_SampleTime_15Cycles); // 4 - ADC_IND_SENS2
|
||||||
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles); // 7 - ADC_IND_EXT2
|
||||||
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_14, 4, ADC_SampleTime_15Cycles); // 10 - ADC_IND_TEMP_MOTOR
|
||||||
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_9, 5, ADC_SampleTime_15Cycles); // 13 - ADC_IND_TEMP_MOS_3
|
||||||
|
|
||||||
|
// ADC3 regular channels - only a subset of channels avaliable
|
||||||
|
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles); // 2 - ADC_IND_CURR3
|
||||||
|
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 2, ADC_SampleTime_15Cycles); // 5 - ADC_IND_SENS3
|
||||||
|
ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 3, ADC_SampleTime_15Cycles); // 8 - ADC_IND_VIN_SENS
|
||||||
|
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles); // 11 - ADC_IND_TEMP_MOS
|
||||||
|
ADC_RegularChannelConfig(ADC3, ADC_Channel_14, 5, ADC_SampleTime_15Cycles); // 14 - UNUSED
|
||||||
|
|
||||||
|
|
||||||
|
// Injected channels
|
||||||
|
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles); // ADC_IND_CURR1
|
||||||
|
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles); // ADC_IND_CURR2
|
||||||
|
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles); // ADC_IND_CURR3
|
||||||
|
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles); // ADC_IND_CURR1
|
||||||
|
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles); // ADC_IND_CURR2
|
||||||
|
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles); // ADC_IND_CURR3
|
||||||
|
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles); // ADC_IND_CURR1
|
||||||
|
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles); // ADC_IND_CURR2
|
||||||
|
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles); // ADC_IND_CURR3
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void hw_start_i2c(void) {
|
||||||
|
i2cAcquireBus(&HW_I2C_DEV);
|
||||||
|
|
||||||
|
if (!i2c_running) {
|
||||||
|
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);
|
||||||
|
i2c_running = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
i2cReleaseBus(&HW_I2C_DEV);
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_stop_i2c(void) {
|
||||||
|
i2cAcquireBus(&HW_I2C_DEV);
|
||||||
|
|
||||||
|
if (i2c_running) {
|
||||||
|
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
|
||||||
|
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
|
||||||
|
|
||||||
|
i2cStop(&HW_I2C_DEV);
|
||||||
|
i2c_running = false;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
i2cReleaseBus(&HW_I2C_DEV);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Try to restore the i2c bus
|
||||||
|
*/
|
||||||
|
void hw_try_restore_i2c(void) {
|
||||||
|
if (i2c_running) {
|
||||||
|
i2cAcquireBus(&HW_I2C_DEV);
|
||||||
|
|
||||||
|
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
|
||||||
|
PAL_STM32_OTYPE_OPENDRAIN |
|
||||||
|
PAL_STM32_OSPEED_MID1 |
|
||||||
|
PAL_STM32_PUDR_PULLUP);
|
||||||
|
|
||||||
|
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||||
|
PAL_STM32_OTYPE_OPENDRAIN |
|
||||||
|
PAL_STM32_OSPEED_MID1 |
|
||||||
|
PAL_STM32_PUDR_PULLUP);
|
||||||
|
|
||||||
|
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||||
|
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||||
|
|
||||||
|
chThdSleep(1);
|
||||||
|
|
||||||
|
for(int i = 0;i < 16;i++) {
|
||||||
|
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Generate start then stop condition
|
||||||
|
palClearPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
HW_I2C_DEV.state = I2C_STOP;
|
||||||
|
i2cStart(&HW_I2C_DEV, &i2cfg);
|
||||||
|
|
||||||
|
i2cReleaseBus(&HW_I2C_DEV);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float hw_a200s_get_temp(void) {
|
||||||
|
float t1 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
|
||||||
|
float t2 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_2]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
|
||||||
|
float t3 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_3]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
|
||||||
|
float res = 0.0;
|
||||||
|
|
||||||
|
if (t1 > t2 && t1 > t3) {
|
||||||
|
res = t1;
|
||||||
|
} else if (t2 > t1 && t2 > t3) {
|
||||||
|
res = t2;
|
||||||
|
} else {
|
||||||
|
res = t3;
|
||||||
|
}
|
||||||
|
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool hw_a200s_hardware_handshake(void) {
|
||||||
|
uint8_t rxb[1];
|
||||||
|
|
||||||
|
// Request handshake code, needs to wait for the i2c to be setup by the imu driver
|
||||||
|
|
||||||
|
if(imu_startup_done()) {
|
||||||
|
if(i2c_bb_tx_rx(imu_get_i2c(), ATTINY3216_ADDR, NULL, 0, rxb, 1)) {
|
||||||
|
if(rxb[0] == ATTINY3216_HANDSHAKE_REPLY) {
|
||||||
|
// OK
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
chThdSleep(10); // Small delay
|
||||||
|
|
||||||
|
|
||||||
|
if(i2c_bb_tx_rx(imu_get_i2c(), ATTINY1616_ADDR, NULL, 0, rxb, 1)) {
|
||||||
|
if(rxb[0] == ATTINY1616_HANDSHAKE_REPLY) {
|
||||||
|
// OK
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool hw_a200s_drv_fault_check(void) {
|
||||||
|
return (palReadPad(GPIOB, 12) || !drv_handshake_complete);
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_a200s_reset_faults(void) {
|
||||||
|
// Send reset command to logger, needs to wait for the i2c to be setup by the imu driver
|
||||||
|
if(imu_startup_done()) {
|
||||||
|
// Only reset the fault if the hardware protections are working.
|
||||||
|
if(hw_a200s_hardware_handshake()) {
|
||||||
|
uint8_t txb[1];
|
||||||
|
|
||||||
|
// Setup current limit digipots at startup they will default to 0A
|
||||||
|
hw_a200s_set_hardware_current_limits();
|
||||||
|
|
||||||
|
// Clear latches
|
||||||
|
txb[0] = 0x53;
|
||||||
|
i2c_bb_tx_rx(imu_get_i2c(), ATTINY3216_ADDR, txb, 1, NULL, 0);
|
||||||
|
|
||||||
|
txb[0] = 0x54;
|
||||||
|
i2c_bb_tx_rx(imu_get_i2c(), ATTINY1616_ADDR, txb, 1, NULL, 0);
|
||||||
|
|
||||||
|
// ATTiny should now have released the gate drivers for us to use
|
||||||
|
drv_handshake_complete = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_a200s_aux(bool enable){
|
||||||
|
static int state = false; // only send changes to attiny1616 to avoid hogging the bus
|
||||||
|
|
||||||
|
if(state != enable)
|
||||||
|
{
|
||||||
|
uint8_t txb[1];
|
||||||
|
|
||||||
|
if(imu_startup_done()) // needs to wait for the i2c to be setup by the imu driver
|
||||||
|
{
|
||||||
|
if(enable)
|
||||||
|
{
|
||||||
|
txb[0] = 0x21;
|
||||||
|
} else {
|
||||||
|
txb[0] = 0x20;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(i2c_bb_tx_rx(imu_get_i2c(), ATTINY1616_ADDR, txb, 1, NULL, 0))
|
||||||
|
{
|
||||||
|
state = enable;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void hw_a200s_set_hardware_current_limits(void){
|
||||||
|
|
||||||
|
uint8_t txb[2];
|
||||||
|
|
||||||
|
if(imu_startup_done()) // needs to wait for the i2c to be setup by the imu driver
|
||||||
|
{
|
||||||
|
// Digipots have 128 positions, default is midpoint
|
||||||
|
// Each position is 3.3 / ((0.0002 / 3) * 20) = 2475 / 128 = 19.34A
|
||||||
|
// So current / 19.34 = trip value position
|
||||||
|
// Midpoint is 64
|
||||||
|
int channel_digipot_position = 64.0f + ceilf(HW_PROTECTION_CURR_TRIP_CHANNEL / 19.34f);
|
||||||
|
int diode_digipot_position = 64.0f - ceilf(HW_PROTECTION_CURR_TRIP_DIODE / 19.34f);
|
||||||
|
|
||||||
|
utils_truncate_number_int(&channel_digipot_position, 64, 127);
|
||||||
|
utils_truncate_number_int(&diode_digipot_position, 0, 64);
|
||||||
|
|
||||||
|
txb[0] = 0x00; // Wiper Position register
|
||||||
|
txb[1] = (unsigned)channel_digipot_position;
|
||||||
|
if(i2c_bb_tx_rx(imu_get_i2c(), TPL0401A_10DCKR_ADDR, txb, 2, NULL, 0))
|
||||||
|
{
|
||||||
|
// Success
|
||||||
|
}
|
||||||
|
|
||||||
|
txb[1] = (unsigned)diode_digipot_position;
|
||||||
|
if(i2c_bb_tx_rx(imu_get_i2c(), TPL0401B_10DCKR_ADDR, txb, 2, NULL, 0))
|
||||||
|
{
|
||||||
|
// Success
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static void terminal_cmd_doublepulse(int argc, const char** argv)
|
||||||
|
{
|
||||||
|
(void)argc;
|
||||||
|
(void)argv;
|
||||||
|
|
||||||
|
int preface, pulse1, breaktime, pulse2;
|
||||||
|
int utick;
|
||||||
|
int deadtime = -1;
|
||||||
|
|
||||||
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||||
|
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||||
|
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
|
||||||
|
|
||||||
|
if (argc < 5) {
|
||||||
|
commands_printf("Usage: double_pulse <preface> <pulse1> <break> <pulse2> [deadtime]");
|
||||||
|
commands_printf(" preface: idle time in us");
|
||||||
|
commands_printf(" pulse1: high time of pulse 1 in us");
|
||||||
|
commands_printf(" break: break between pulses in us");
|
||||||
|
commands_printf(" pulse2: high time of pulse 2 in us");
|
||||||
|
commands_printf(" deadtime: overwrite deadtime, in ns");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
sscanf(argv[1], "%d", &preface);
|
||||||
|
sscanf(argv[2], "%d", &pulse1);
|
||||||
|
sscanf(argv[3], "%d", &breaktime);
|
||||||
|
sscanf(argv[4], "%d", &pulse2);
|
||||||
|
if (argc == 6) {
|
||||||
|
sscanf(argv[5], "%d", &deadtime);
|
||||||
|
}
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
|
utick = (int)(SYSTEM_CORE_CLOCK / 1000000);
|
||||||
|
mcpwm_deinit();
|
||||||
|
mcpwm_foc_deinit();
|
||||||
|
gpdrive_deinit();
|
||||||
|
|
||||||
|
TIM_Cmd(TIM1, DISABLE);
|
||||||
|
TIM_Cmd(TIM4, DISABLE);
|
||||||
|
//TIM4 als Trigger Timer
|
||||||
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
|
||||||
|
|
||||||
|
TIM_TimeBaseStructure.TIM_Period = (SYSTEM_CORE_CLOCK / 20000);
|
||||||
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
||||||
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||||
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
|
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
||||||
|
TIM_SelectMasterSlaveMode(TIM4, TIM_MasterSlaveMode_Enable);
|
||||||
|
TIM_SelectOutputTrigger(TIM4, TIM_TRGOSource_Enable);
|
||||||
|
TIM4->CNT = 0;
|
||||||
|
|
||||||
|
// TIM1
|
||||||
|
// TIM1 clock enable
|
||||||
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
|
||||||
|
|
||||||
|
// Time Base configuration
|
||||||
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
||||||
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
|
TIM_TimeBaseStructure.TIM_Period = (preface + pulse1) * utick;
|
||||||
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||||
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
||||||
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
||||||
|
|
||||||
|
// Channel 1, 2 and 3 Configuration in PWM mode
|
||||||
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
||||||
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
||||||
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
|
||||||
|
TIM_OCInitStructure.TIM_Pulse = preface * utick;
|
||||||
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
|
||||||
|
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
|
||||||
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
||||||
|
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
|
||||||
|
|
||||||
|
TIM_OC1Init(TIM1, &TIM_OCInitStructure);
|
||||||
|
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
|
||||||
|
TIM_OC2Init(TIM1, &TIM_OCInitStructure);
|
||||||
|
TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
|
||||||
|
TIM_OC3Init(TIM1, &TIM_OCInitStructure);
|
||||||
|
TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
|
||||||
|
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM2);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
|
||||||
|
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_Inactive);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Enable);
|
||||||
|
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
|
||||||
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
||||||
|
|
||||||
|
|
||||||
|
// Automatic Output enable, Break, dead time and lock configuration
|
||||||
|
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
||||||
|
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
||||||
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
||||||
|
if (deadtime < 0) {
|
||||||
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
|
||||||
|
} else {
|
||||||
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(deadtime, SYSTEM_CORE_CLOCK);
|
||||||
|
}
|
||||||
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
||||||
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
||||||
|
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
||||||
|
TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
|
||||||
|
|
||||||
|
TIM_CCPreloadControl(TIM1, ENABLE);
|
||||||
|
TIM_ARRPreloadConfig(TIM1, ENABLE);
|
||||||
|
|
||||||
|
TIM1->CNT = 0;
|
||||||
|
TIM1->EGR = TIM_EGR_UG;
|
||||||
|
|
||||||
|
TIM_SelectSlaveMode(TIM1, TIM_SlaveMode_Trigger);
|
||||||
|
TIM_SelectInputTrigger(TIM1, TIM_TS_ITR3);
|
||||||
|
TIM_SelectOnePulseMode(TIM1, TIM_OPMode_Single);
|
||||||
|
TIM_CtrlPWMOutputs(TIM1, ENABLE);
|
||||||
|
|
||||||
|
TIM_Cmd(TIM1, ENABLE);
|
||||||
|
//Timer 4 triggert Timer 1
|
||||||
|
TIM_Cmd(TIM4, ENABLE);
|
||||||
|
TIM_Cmd(TIM4, DISABLE);
|
||||||
|
TIM1->ARR = (breaktime + pulse2) * utick;
|
||||||
|
TIM1->CCR1 = breaktime * utick;
|
||||||
|
while (TIM1->CNT != 0);
|
||||||
|
TIM_Cmd(TIM4, ENABLE);
|
||||||
|
|
||||||
|
chThdSleepMilliseconds(1);
|
||||||
|
TIM_CtrlPWMOutputs(TIM1, DISABLE);
|
||||||
|
mc_configuration* mcconf = mempools_alloc_mcconf();
|
||||||
|
*mcconf = *mc_interface_get_configuration();
|
||||||
|
|
||||||
|
switch (mcconf->motor_type) {
|
||||||
|
case MOTOR_TYPE_BLDC:
|
||||||
|
case MOTOR_TYPE_DC:
|
||||||
|
mcpwm_init(mcconf);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_TYPE_FOC:
|
||||||
|
mcpwm_foc_init(mcconf, mcconf);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_TYPE_GPD:
|
||||||
|
gpdrive_init(mcconf);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
commands_printf("Done");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,272 @@
|
||||||
|
/*
|
||||||
|
Copyright 2018 Benjamin Vedder benjamin@vedder.se
|
||||||
|
|
||||||
|
This file is part of the VESC firmware.
|
||||||
|
|
||||||
|
The VESC firmware 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.
|
||||||
|
|
||||||
|
The VESC firmware 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef HW_A200S_V41_CORE_H_
|
||||||
|
#define HW_A200S_V41_CORE_H_
|
||||||
|
|
||||||
|
// HW properties
|
||||||
|
#define HW_HAS_3_SHUNTS
|
||||||
|
#define INVERTED_SHUNT_POLARITY
|
||||||
|
#define HW_HAS_PHASE_FILTERS
|
||||||
|
#define HW_USE_25MHZ_EXT_CLOCK
|
||||||
|
#define HW_USE_ALTERNATIVE_DC_CAL
|
||||||
|
|
||||||
|
#define HW_ADC_CHANNELS 15
|
||||||
|
#define HW_ADC_INJ_CHANNELS 3
|
||||||
|
#define HW_ADC_NBR_CONV 5
|
||||||
|
|
||||||
|
// ADC Indexes - refer to .c for descriptions
|
||||||
|
#define ADC_IND_CURR1 0
|
||||||
|
#define ADC_IND_CURR2 1
|
||||||
|
#define ADC_IND_CURR3 2
|
||||||
|
|
||||||
|
#define ADC_IND_VIN_SENS 8
|
||||||
|
#define ADC_IND_SENS1 3
|
||||||
|
#define ADC_IND_SENS2 4
|
||||||
|
#define ADC_IND_SENS3 5
|
||||||
|
|
||||||
|
#define ADC_IND_EXT 6
|
||||||
|
#define ADC_IND_EXT2 7
|
||||||
|
//#define ADC_IND_EXT3
|
||||||
|
|
||||||
|
#define ADC_IND_TEMP_MOS 11
|
||||||
|
#define ADC_IND_TEMP_MOS_2 12
|
||||||
|
#define ADC_IND_TEMP_MOS_3 13
|
||||||
|
#define ADC_IND_TEMP_MOTOR 10
|
||||||
|
|
||||||
|
#define ADC_IND_VREFINT 9
|
||||||
|
|
||||||
|
|
||||||
|
// ADC macros and settings
|
||||||
|
|
||||||
|
// Component parameters (can be overridden)
|
||||||
|
#ifndef V_REG
|
||||||
|
#define V_REG 3.30
|
||||||
|
#endif
|
||||||
|
#ifndef VIN_R1
|
||||||
|
#define VIN_R1 100000.0
|
||||||
|
#endif
|
||||||
|
#ifndef VIN_R2
|
||||||
|
#define VIN_R2 3160.0
|
||||||
|
#endif
|
||||||
|
#ifndef CURRENT_AMP_GAIN
|
||||||
|
#define CURRENT_AMP_GAIN 20.0
|
||||||
|
#endif
|
||||||
|
#ifndef CURRENT_SHUNT_RES
|
||||||
|
#define CURRENT_SHUNT_RES (0.0002 / 3)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
// Input voltage
|
||||||
|
#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
|
||||||
|
|
||||||
|
// NTC Termistors
|
||||||
|
#define NTC_RES(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Mos temp sensor on low side
|
||||||
|
|
||||||
|
#define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side
|
||||||
|
#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15)
|
||||||
|
|
||||||
|
#define NTC_TEMP(adc_ind) hw_a200s_get_temp()
|
||||||
|
#define NTC_TEMP_MOS1() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
|
||||||
|
#define NTC_TEMP_MOS2() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_2]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
|
||||||
|
#define NTC_TEMP_MOS3() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_3]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
|
||||||
|
|
||||||
|
// Voltage on ADC channel
|
||||||
|
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
||||||
|
|
||||||
|
|
||||||
|
#define LED_GREEN_GPIO GPIOB
|
||||||
|
#define LED_GREEN_PIN 5
|
||||||
|
#define LED_RED_GPIO GPIOB
|
||||||
|
#define LED_RED_PIN 7
|
||||||
|
|
||||||
|
#define LED_GREEN_ON() palSetPad(LED_GREEN_GPIO, LED_GREEN_PIN)
|
||||||
|
#define LED_GREEN_OFF() palClearPad(LED_GREEN_GPIO, LED_GREEN_PIN)
|
||||||
|
#define LED_RED_ON() palSetPad(LED_RED_GPIO, LED_RED_PIN)
|
||||||
|
#define LED_RED_OFF() palClearPad(LED_RED_GPIO, LED_RED_PIN)
|
||||||
|
|
||||||
|
// Aux - TCKE812NA,RF - Aux output controlled by I2C
|
||||||
|
#define AUX_ON() hw_a200s_aux(1)
|
||||||
|
#define AUX_OFF() hw_a200s_aux(0)
|
||||||
|
|
||||||
|
// Phase filter
|
||||||
|
#define PHASE_FILTER_OFF() palSetPad(GPIOC, 9); palSetPad(GPIOC, 13); palSetPad(GPIOC, 14)
|
||||||
|
#define PHASE_FILTER_ON() palClearPad(GPIOC, 9); palClearPad(GPIOC, 13); palClearPad(GPIOC, 14)
|
||||||
|
|
||||||
|
// COMM-port ADC GPIOs
|
||||||
|
#define HW_ADC_EXT_GPIO GPIOA
|
||||||
|
#define HW_ADC_EXT_PIN 5
|
||||||
|
#define HW_ADC_EXT2_GPIO GPIOA
|
||||||
|
#define HW_ADC_EXT2_PIN 6
|
||||||
|
|
||||||
|
// UART Peripheral - Comm port
|
||||||
|
#define HW_UART_DEV SD3
|
||||||
|
#define HW_UART_GPIO_AF GPIO_AF_USART3
|
||||||
|
#define HW_UART_TX_PORT GPIOB
|
||||||
|
#define HW_UART_TX_PIN 10
|
||||||
|
#define HW_UART_RX_PORT GPIOB
|
||||||
|
#define HW_UART_RX_PIN 11
|
||||||
|
|
||||||
|
// SPI pins
|
||||||
|
#define HW_SPI_DEV SPID1
|
||||||
|
#define HW_SPI_GPIO_AF GPIO_AF_SPI1
|
||||||
|
#define HW_SPI_PORT_NSS GPIOB
|
||||||
|
#define HW_SPI_PIN_NSS 11
|
||||||
|
#define HW_SPI_PORT_SCK GPIOA
|
||||||
|
#define HW_SPI_PIN_SCK 5
|
||||||
|
#define HW_SPI_PORT_MOSI GPIOA
|
||||||
|
#define HW_SPI_PIN_MOSI 7
|
||||||
|
#define HW_SPI_PORT_MISO GPIOA
|
||||||
|
#define HW_SPI_PIN_MISO 6
|
||||||
|
|
||||||
|
// 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
|
||||||
|
|
||||||
|
// Hall/encoder pins
|
||||||
|
#define HW_HALL_ENC_GPIO1 GPIOC
|
||||||
|
#define HW_HALL_ENC_PIN1 6
|
||||||
|
#define HW_HALL_ENC_GPIO2 GPIOC
|
||||||
|
#define HW_HALL_ENC_PIN2 7
|
||||||
|
#define HW_HALL_ENC_GPIO3 GPIOC
|
||||||
|
#define HW_HALL_ENC_PIN3 8
|
||||||
|
#define HW_ENC_TIM TIM3
|
||||||
|
#define HW_ENC_TIM_AF GPIO_AF_TIM3
|
||||||
|
#define HW_ENC_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
|
||||||
|
#define HW_ENC_EXTI_PORTSRC EXTI_PortSourceGPIOC
|
||||||
|
#define HW_ENC_EXTI_PINSRC EXTI_PinSource8
|
||||||
|
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
|
||||||
|
#define HW_ENC_EXTI_LINE EXTI_Line8
|
||||||
|
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
|
||||||
|
#define HW_ENC_TIM_ISR_CH TIM3_IRQn
|
||||||
|
#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
|
||||||
|
|
||||||
|
// Permanent UART Peripheral (for Logger)
|
||||||
|
#define HW_UART_P_BAUD 100000
|
||||||
|
#define HW_UART_P_DEV SD4
|
||||||
|
#define HW_UART_P_GPIO_AF GPIO_AF_UART4
|
||||||
|
#define HW_UART_P_TX_PORT GPIOC
|
||||||
|
#define HW_UART_P_TX_PIN 10
|
||||||
|
#define HW_UART_P_RX_PORT GPIOC
|
||||||
|
#define HW_UART_P_RX_PIN 11
|
||||||
|
|
||||||
|
// ICU Peripheral for servo decoding
|
||||||
|
#define HW_USE_SERVO_TIM4
|
||||||
|
#define HW_ICU_TIMER TIM4
|
||||||
|
#define HW_ICU_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
|
||||||
|
#define HW_ICU_DEV ICUD4
|
||||||
|
#define HW_ICU_CHANNEL ICU_CHANNEL_1
|
||||||
|
#define HW_ICU_GPIO_AF GPIO_AF_TIM4
|
||||||
|
#define HW_ICU_GPIO GPIOB
|
||||||
|
#define HW_ICU_PIN 6
|
||||||
|
|
||||||
|
// I2C for IMU, logger and output control
|
||||||
|
#define LSM6DS3_SDA_GPIO GPIOC
|
||||||
|
#define LSM6DS3_SDA_PIN 12
|
||||||
|
#define LSM6DS3_SCL_GPIO GPIOA
|
||||||
|
#define LSM6DS3_SCL_PIN 4
|
||||||
|
#define ATTINY1616_ADDR 0X68 // Low side body diode OC and logger
|
||||||
|
#define ATTINY3216_ADDR 0X69 // Low side Mosfet Channel OC and Aux output control
|
||||||
|
#define TPL0401A_10DCKR_ADDR 0X2E // Low side Mosfet Channel OC limit digipot
|
||||||
|
#define TPL0401B_10DCKR_ADDR 0X3E // Low side body diode OC limit digipot
|
||||||
|
#define ATTINY3216_HANDSHAKE_REPLY 0x71
|
||||||
|
#define ATTINY1616_HANDSHAKE_REPLY 0x73
|
||||||
|
|
||||||
|
|
||||||
|
// Hardware protection
|
||||||
|
#define HW_RESET_DRV_FAULTS() hw_a200s_reset_faults()
|
||||||
|
#define ENABLE_GATE() palClearPad(GPIOC, 5)
|
||||||
|
#define DISABLE_GATE() palSetPad(GPIOC, 5)
|
||||||
|
#define IS_DRV_FAULT() hw_a200s_drv_fault_check()
|
||||||
|
#define HW_PROTECTION_CURR_TRIP_CHANNEL 900.0f
|
||||||
|
#define HW_PROTECTION_CURR_TRIP_DIODE 1000.0f // Seems to be about 45% of this (i.e. this trips with ~450A flowing before the fet turns off)
|
||||||
|
|
||||||
|
#define BRK_GPIO GPIOB
|
||||||
|
#define BRK_PIN 12
|
||||||
|
|
||||||
|
// Measurement macros
|
||||||
|
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
|
||||||
|
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]
|
||||||
|
#define ADC_V_L3 ADC_Value[ADC_IND_SENS3]
|
||||||
|
#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2)
|
||||||
|
|
||||||
|
// Macros
|
||||||
|
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
|
||||||
|
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
||||||
|
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||||
|
|
||||||
|
// Default setting overrides
|
||||||
|
#define MCCONF_L_MIN_VOLTAGE 14.0 // Minimum input voltage
|
||||||
|
|
||||||
|
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
|
||||||
|
|
||||||
|
#define MCCONF_FOC_F_ZV 30000.0
|
||||||
|
|
||||||
|
#define HW_LIM_FOC_CTRL_LOOP_FREQ 5000.0, 25000.0 //Limit to 50kHz max
|
||||||
|
|
||||||
|
#define MCCONF_L_MAX_ABS_CURRENT 200.0 // The maximum absolute current above which a fault is generated
|
||||||
|
|
||||||
|
#define MCCONF_FOC_SAMPLE_V0_V7 false // Run control loop in both v0 and v7 (requires phase shunts)
|
||||||
|
|
||||||
|
#define MCCONF_L_CURRENT_MAX 100.0 // Current limit in Amperes (Upper)
|
||||||
|
|
||||||
|
#define MCCONF_L_CURRENT_MIN -100.0 // Current limit in Amperes (Lower)
|
||||||
|
|
||||||
|
#define MCCONF_L_SLOW_ABS_OVERCURRENT false // Use the raw current for the overcurrent fault detection
|
||||||
|
|
||||||
|
#define MCCONF_L_IN_CURRENT_MAX 20.0 // Input current limit in Amperes (Upper)
|
||||||
|
|
||||||
|
#define MCCONF_L_IN_CURRENT_MIN -5.0 // Input current limit in Amperes (Lower)
|
||||||
|
|
||||||
|
#define MCCONF_M_MOTOR_TEMP_SENS_TYPE TEMP_SENSOR_DISABLED // Motor Temperature Sensor Type
|
||||||
|
|
||||||
|
#define MCCONF_FOC_OFFSETS_CAL_ON_BOOT false // Don't Measure offsets every boot, it is done once at motor setup
|
||||||
|
|
||||||
|
// Setting limits
|
||||||
|
#define HW_LIM_CURRENT -300.0, 300.0
|
||||||
|
#define HW_LIM_CURRENT_IN -180.0, 180.0
|
||||||
|
#define HW_LIM_CURRENT_ABS 0.0, 450.0
|
||||||
|
#define HW_LIM_VIN -1.0, 90.0
|
||||||
|
#define HW_LIM_ERPM -200e3, 200e3
|
||||||
|
#define HW_LIM_DUTY_MIN 0.0, 0.1
|
||||||
|
#define HW_LIM_DUTY_MAX 0.0, 0.95
|
||||||
|
#define HW_LIM_TEMP_FET -40.0, 110.0
|
||||||
|
#ifndef MCCONF_L_MAX_VOLTAGE
|
||||||
|
#define MCCONF_L_MAX_VOLTAGE 20.0 * 4.2 + 5.0 // Maximum input voltage
|
||||||
|
#endif
|
||||||
|
#ifndef MCCONF_FOC_DT_US
|
||||||
|
#define MCCONF_FOC_DT_US 0.1 // Microseconds for dead time compensation
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define HW_DEAD_TIME_NSEC 700.0
|
||||||
|
#define HW_NAME "A200S_V41"
|
||||||
|
|
||||||
|
|
||||||
|
// HW-specific functions
|
||||||
|
float hw_a200s_get_temp(void);
|
||||||
|
void hw_a200s_reset_faults(void);
|
||||||
|
bool hw_a200s_hardware_handshake(void);
|
||||||
|
bool hw_a200s_drv_fault_check(void);
|
||||||
|
void hw_a200s_aux(bool);
|
||||||
|
|
||||||
|
#endif /* HW_A200S_V41_CORE_H_ */
|
||||||
11
imu/imu.c
11
imu/imu.c
|
|
@ -262,9 +262,14 @@ void imu_init_bmi160_spi(stm32_gpio_t *nss_gpio, int nss_pin,
|
||||||
void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
|
void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
|
||||||
stm32_gpio_t *scl_gpio, int scl_pin) {
|
stm32_gpio_t *scl_gpio, int scl_pin) {
|
||||||
|
|
||||||
lsm6ds3_init(sda_gpio, sda_pin,
|
m_i2c_bb.sda_gpio = sda_gpio;
|
||||||
scl_gpio, scl_pin,
|
m_i2c_bb.sda_pin = sda_pin;
|
||||||
m_thd_work_area, sizeof(m_thd_work_area));
|
m_i2c_bb.scl_gpio = scl_gpio;
|
||||||
|
m_i2c_bb.scl_pin = scl_pin;
|
||||||
|
m_i2c_bb.rate = I2C_BB_RATE_400K;
|
||||||
|
i2c_bb_init(&m_i2c_bb);
|
||||||
|
|
||||||
|
lsm6ds3_init(&m_i2c_bb, m_thd_work_area, sizeof(m_thd_work_area));
|
||||||
lsm6ds3_set_read_callback(imu_read_callback);
|
lsm6ds3_set_read_callback(imu_read_callback);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -27,7 +27,7 @@
|
||||||
|
|
||||||
|
|
||||||
static thread_t *lsm6ds3_thread_ref = NULL;
|
static thread_t *lsm6ds3_thread_ref = NULL;
|
||||||
static i2c_bb_state m_i2c_bb;
|
static i2c_bb_state *m_i2c_bb;
|
||||||
static volatile uint16_t lsm6ds3_addr;
|
static volatile uint16_t lsm6ds3_addr;
|
||||||
static int rate_hz = 1000;
|
static int rate_hz = 1000;
|
||||||
static IMU_FILTER filter;
|
static IMU_FILTER filter;
|
||||||
|
|
@ -48,29 +48,23 @@ void lsm6ds3_set_filter(IMU_FILTER f) {
|
||||||
filter = f;
|
filter = f;
|
||||||
}
|
}
|
||||||
|
|
||||||
void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
void lsm6ds3_init(i2c_bb_state *i2c_state,
|
||||||
stm32_gpio_t *scl_gpio, int scl_pin,
|
|
||||||
stkalign_t *work_area, size_t work_area_size) {
|
stkalign_t *work_area, size_t work_area_size) {
|
||||||
|
|
||||||
read_callback = 0;
|
read_callback = 0;
|
||||||
|
|
||||||
m_i2c_bb.sda_gpio = sda_gpio;
|
m_i2c_bb = i2c_state;
|
||||||
m_i2c_bb.sda_pin = sda_pin;
|
|
||||||
m_i2c_bb.scl_gpio = scl_gpio;
|
|
||||||
m_i2c_bb.scl_pin = scl_pin;
|
|
||||||
m_i2c_bb.rate = I2C_BB_RATE_400K;
|
|
||||||
i2c_bb_init(&m_i2c_bb);
|
|
||||||
|
|
||||||
uint8_t txb[2];
|
uint8_t txb[2];
|
||||||
uint8_t rxb[2];
|
uint8_t rxb[2];
|
||||||
|
|
||||||
txb[0] = LSM6DS3_ACC_GYRO_WHO_AM_I_REG;
|
txb[0] = LSM6DS3_ACC_GYRO_WHO_AM_I_REG;
|
||||||
lsm6ds3_addr = LSM6DS3_ACC_GYRO_ADDR_A;
|
lsm6ds3_addr = LSM6DS3_ACC_GYRO_ADDR_A;
|
||||||
bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 1);
|
bool res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 1);
|
||||||
if (!res || (rxb[0] != 0x69 && rxb[0] != 0x6A && rxb[0] != 0x6C)) {
|
if (!res || (rxb[0] != 0x69 && rxb[0] != 0x6A && rxb[0] != 0x6C)) {
|
||||||
commands_printf("LSM6DS3 Address A failed, trying B (rx: %d)", rxb[0]);
|
commands_printf("LSM6DS3 Address A failed, trying B (rx: %d)", rxb[0]);
|
||||||
lsm6ds3_addr = LSM6DS3_ACC_GYRO_ADDR_B;
|
lsm6ds3_addr = LSM6DS3_ACC_GYRO_ADDR_B;
|
||||||
res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 1);
|
res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 1);
|
||||||
if (!res || (rxb[0] != 0x69 && rxb[0] != 0x6A && rxb[0] != 0x6C)) {
|
if (!res || (rxb[0] != 0x69 && rxb[0] != 0x6A && rxb[0] != 0x6C)) {
|
||||||
commands_printf("LSM6DS3 Address B failed (rx: %d)", rxb[0]);
|
commands_printf("LSM6DS3 Address B failed (rx: %d)", rxb[0]);
|
||||||
return;
|
return;
|
||||||
|
|
@ -133,7 +127,7 @@ void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
||||||
} else {
|
} else {
|
||||||
txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
|
txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
|
||||||
}
|
}
|
||||||
res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
||||||
if (!res){
|
if (!res){
|
||||||
commands_printf("LSM6DS3 Accel Config FAILED");
|
commands_printf("LSM6DS3 Accel Config FAILED");
|
||||||
return;
|
return;
|
||||||
|
|
@ -163,7 +157,7 @@ void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
||||||
} else {
|
} else {
|
||||||
txb[1] |= LSM6DS3TRC_ACC_GYRO_ODR_G_6660Hz;
|
txb[1] |= LSM6DS3TRC_ACC_GYRO_ODR_G_6660Hz;
|
||||||
}
|
}
|
||||||
res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
||||||
if (!res){
|
if (!res){
|
||||||
commands_printf("LSM6DS3 Gyro Config FAILED");
|
commands_printf("LSM6DS3 Gyro Config FAILED");
|
||||||
return;
|
return;
|
||||||
|
|
@ -183,7 +177,7 @@ void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
||||||
// Standard LSM6DS3 only: Set XL anti-aliasing filter to be manually configured
|
// Standard LSM6DS3 only: Set XL anti-aliasing filter to be manually configured
|
||||||
txb[1] = LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
|
txb[1] = LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
|
||||||
}
|
}
|
||||||
res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
||||||
if (!res){
|
if (!res){
|
||||||
commands_printf("LSM6DS3 ODR Config FAILED");
|
commands_printf("LSM6DS3 ODR Config FAILED");
|
||||||
return;
|
return;
|
||||||
|
|
@ -195,7 +189,7 @@ void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
||||||
#define LSM6DS3TRC_HPCF_XL_ODR9 0x40
|
#define LSM6DS3TRC_HPCF_XL_ODR9 0x40
|
||||||
txb[0] = LSM6DS3_ACC_GYRO_CTRL8_XL;
|
txb[0] = LSM6DS3_ACC_GYRO_CTRL8_XL;
|
||||||
txb[1] = LSM6DS3TRC_LPF2_XL_EN | LSM6DS3TRC_HPCF_XL_ODR9;
|
txb[1] = LSM6DS3TRC_LPF2_XL_EN | LSM6DS3TRC_HPCF_XL_ODR9;
|
||||||
res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
||||||
if (!res) {
|
if (!res) {
|
||||||
commands_printf("LSM6DS3 Accel Low Pass Config FAILED");
|
commands_printf("LSM6DS3 Accel Low Pass Config FAILED");
|
||||||
return;
|
return;
|
||||||
|
|
@ -229,7 +223,7 @@ static uint8_t read_single_reg(uint8_t reg) {
|
||||||
uint8_t rxb[2];
|
uint8_t rxb[2];
|
||||||
|
|
||||||
txb[0] = reg;
|
txb[0] = reg;
|
||||||
bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 2);
|
bool res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 2);
|
||||||
|
|
||||||
if (res) {
|
if (res) {
|
||||||
return rxb[0];
|
return rxb[0];
|
||||||
|
|
@ -272,11 +266,11 @@ static THD_FUNCTION(lsm6ds3_thread, arg) {
|
||||||
// Disable IMU writing to output registers
|
// Disable IMU writing to output registers
|
||||||
txb[0] = LSM6DS3_ACC_GYRO_CTRL3_C;
|
txb[0] = LSM6DS3_ACC_GYRO_CTRL3_C;
|
||||||
txb[1] = LSM6DS3_ACC_GYRO_BDU_BLOCK_UPDATE | LSM6DS3_ACC_GYRO_IF_INC_ENABLED;
|
txb[1] = LSM6DS3_ACC_GYRO_BDU_BLOCK_UPDATE | LSM6DS3_ACC_GYRO_IF_INC_ENABLED;
|
||||||
i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
|
||||||
|
|
||||||
// Read IMU output registers
|
// Read IMU output registers
|
||||||
txb[0] = LSM6DS3_ACC_GYRO_OUTX_L_G;
|
txb[0] = LSM6DS3_ACC_GYRO_OUTX_L_G;
|
||||||
bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 12);
|
bool res = i2c_bb_tx_rx(m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 12);
|
||||||
|
|
||||||
// Parse 6 axis values
|
// Parse 6 axis values
|
||||||
float gx = (float)((int16_t)((uint16_t)rxb[1] << 8) + rxb[0]) * 4.375 * (2000 / 125) / 1000;
|
float gx = (float)((int16_t)((uint16_t)rxb[1] << 8) + rxb[0]) * 4.375 * (2000 / 125) / 1000;
|
||||||
|
|
|
||||||
|
|
@ -36,11 +36,11 @@
|
||||||
#include "ch.h"
|
#include "ch.h"
|
||||||
#include "hal.h"
|
#include "hal.h"
|
||||||
|
|
||||||
|
#include "i2c_bb.h"
|
||||||
|
|
||||||
void lsm6ds3_set_rate_hz(int hz);
|
void lsm6ds3_set_rate_hz(int hz);
|
||||||
void lsm6ds3_set_filter(IMU_FILTER f);
|
void lsm6ds3_set_filter(IMU_FILTER f);
|
||||||
void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
|
void lsm6ds3_init(i2c_bb_state *i2c_state, stkalign_t *work_area, size_t work_area_size);
|
||||||
stm32_gpio_t *scl_gpio, int scl_pin,
|
|
||||||
stkalign_t *work_area, size_t work_area_size);
|
|
||||||
void lsm6ds3_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
|
void lsm6ds3_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
|
||||||
void lsm6ds3_stop(void);
|
void lsm6ds3_stop(void);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -454,7 +454,9 @@ void mcpwm_foc_init(mc_configuration *conf_m1, mc_configuration *conf_m2) {
|
||||||
CURRENT_FILTER_ON_M2();
|
CURRENT_FILTER_ON_M2();
|
||||||
ENABLE_GATE();
|
ENABLE_GATE();
|
||||||
DCCAL_OFF();
|
DCCAL_OFF();
|
||||||
|
#ifdef HW_USE_ALTERNATIVE_DC_CAL
|
||||||
|
m_dccal_done = true;
|
||||||
|
#else
|
||||||
if (m_motor_1.m_conf->foc_offsets_cal_on_boot) {
|
if (m_motor_1.m_conf->foc_offsets_cal_on_boot) {
|
||||||
systime_t cal_start_time = chVTGetSystemTimeX();
|
systime_t cal_start_time = chVTGetSystemTimeX();
|
||||||
float cal_start_timeout = 10.0;
|
float cal_start_timeout = 10.0;
|
||||||
|
|
@ -531,7 +533,7 @@ void mcpwm_foc_init(mc_configuration *conf_m1, mc_configuration *conf_m2) {
|
||||||
} else {
|
} else {
|
||||||
m_dccal_done = true;
|
m_dccal_done = true;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
// Start threads
|
// Start threads
|
||||||
timer_thd_stop = false;
|
timer_thd_stop = false;
|
||||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||||
|
|
@ -2274,6 +2276,7 @@ int mcpwm_foc_hall_detect(float current, uint8_t *hall_table, bool *result) {
|
||||||
* 1: Success
|
* 1: Success
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
#ifndef HW_USE_ALTERNATIVE_DC_CAL
|
||||||
int mcpwm_foc_dc_cal(bool cal_undriven) {
|
int mcpwm_foc_dc_cal(bool cal_undriven) {
|
||||||
// Wait max 5 seconds for DRV-fault to go away
|
// Wait max 5 seconds for DRV-fault to go away
|
||||||
int cnt = 0;
|
int cnt = 0;
|
||||||
|
|
@ -2484,6 +2487,119 @@ int mcpwm_foc_dc_cal(bool cal_undriven) {
|
||||||
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
#else
|
||||||
|
// WARNING: This calibration routine can only be run when the motor is not spinning.
|
||||||
|
// For low side shunt hardware with high capacitance mosfets this works a lot better
|
||||||
|
int mcpwm_foc_dc_cal(bool cal_undriven) {
|
||||||
|
// Wait max 5 seconds for DRV-fault to go away
|
||||||
|
int cnt = 0;
|
||||||
|
while(IS_DRV_FAULT()){
|
||||||
|
chThdSleepMilliseconds(1);
|
||||||
|
cnt++;
|
||||||
|
if (cnt > 5000) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
chThdSleepMilliseconds(1000);
|
||||||
|
|
||||||
|
// Disable timeout
|
||||||
|
systime_t tout = timeout_get_timeout_msec();
|
||||||
|
float tout_c = timeout_get_brake_current();
|
||||||
|
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
|
||||||
|
timeout_reset();
|
||||||
|
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
|
||||||
|
|
||||||
|
// Measure driven offsets
|
||||||
|
const float samples = 1000.0;
|
||||||
|
float current_sum[3] = {0.0, 0.0, 0.0};
|
||||||
|
float voltage_sum[3] = {0.0, 0.0, 0.0};
|
||||||
|
|
||||||
|
TIMER_UPDATE_DUTY_M1(TIM1->ARR / 2, TIM1->ARR / 2, TIM1->ARR / 2);
|
||||||
|
|
||||||
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
||||||
|
PHASE_FILTER_ON();
|
||||||
|
|
||||||
|
// Start PWM on all phases at 50% to get a V0 measurement
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
|
||||||
|
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_2, TIM_OCMode_PWM1);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_2, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_2, TIM_CCxN_Enable);
|
||||||
|
|
||||||
|
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM1);
|
||||||
|
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
|
||||||
|
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
|
||||||
|
|
||||||
|
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
|
||||||
|
|
||||||
|
chThdSleep(1);
|
||||||
|
|
||||||
|
for (float i = 0; i < samples; i++) {
|
||||||
|
current_sum[0] += m_motor_1.m_currents_adc[0];
|
||||||
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1);
|
||||||
|
current_sum[1] += m_motor_1.m_currents_adc[1];
|
||||||
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2);
|
||||||
|
current_sum[2] += m_motor_1.m_currents_adc[2];
|
||||||
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3);
|
||||||
|
chThdSleep(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
||||||
|
|
||||||
|
m_motor_1.m_conf->foc_offsets_current[0] = current_sum[0] / samples;
|
||||||
|
m_motor_1.m_conf->foc_offsets_current[1] = current_sum[1] / samples;
|
||||||
|
m_motor_1.m_conf->foc_offsets_current[2] = current_sum[2] / samples;
|
||||||
|
|
||||||
|
voltage_sum[0] /= samples;
|
||||||
|
voltage_sum[1] /= samples;
|
||||||
|
voltage_sum[2] /= samples;
|
||||||
|
float v_avg = (voltage_sum[0] + voltage_sum[1] + voltage_sum[2]) / 3.0;
|
||||||
|
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage[0] = voltage_sum[0] - v_avg;
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage[1] = voltage_sum[1] - v_avg;
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage[2] = voltage_sum[2] - v_avg;
|
||||||
|
|
||||||
|
// Measure undriven offsets
|
||||||
|
|
||||||
|
if (cal_undriven) {
|
||||||
|
chThdSleepMilliseconds(10);
|
||||||
|
|
||||||
|
voltage_sum[0] = 0.0; voltage_sum[1] = 0.0; voltage_sum[2] = 0.0;
|
||||||
|
|
||||||
|
for (float i = 0;i < samples;i++) {
|
||||||
|
v_avg = (ADC_VOLTS(ADC_IND_SENS1) + ADC_VOLTS(ADC_IND_SENS2) + ADC_VOLTS(ADC_IND_SENS3)) / 3.0;
|
||||||
|
voltage_sum[0] += ADC_VOLTS(ADC_IND_SENS1) - v_avg;
|
||||||
|
voltage_sum[1] += ADC_VOLTS(ADC_IND_SENS2) - v_avg;
|
||||||
|
voltage_sum[2] += ADC_VOLTS(ADC_IND_SENS3) - v_avg;
|
||||||
|
|
||||||
|
chThdSleep(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
stop_pwm_hw((motor_all_state_t*)&m_motor_1);
|
||||||
|
|
||||||
|
voltage_sum[0] /= samples;
|
||||||
|
voltage_sum[1] /= samples;
|
||||||
|
voltage_sum[2] /= samples;
|
||||||
|
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[0] = voltage_sum[0];
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[1] = voltage_sum[1];
|
||||||
|
m_motor_1.m_conf->foc_offsets_voltage_undriven[2] = voltage_sum[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: Make sure that offsets are no more than e.g. 5%, as larger values indicate hardware problems.
|
||||||
|
|
||||||
|
// Enable timeout
|
||||||
|
timeout_configure(tout, tout_c, tout_ksw);
|
||||||
|
mc_interface_unlock();
|
||||||
|
|
||||||
|
m_dccal_done = true;
|
||||||
|
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void mcpwm_foc_print_state(void) {
|
void mcpwm_foc_print_state(void) {
|
||||||
commands_printf("Mod d: %.2f", (double)get_motor_now()->m_motor_state.mod_d);
|
commands_printf("Mod d: %.2f", (double)get_motor_now()->m_motor_state.mod_d);
|
||||||
|
|
|
||||||
|
|
@ -64,6 +64,7 @@ package_dict["A200S_V2.1"] = [['a200s_v2.1', default_name]]
|
||||||
package_dict["A200S_V2.2"] = [['a200s_v2.2', default_name]]
|
package_dict["A200S_V2.2"] = [['a200s_v2.2', default_name]]
|
||||||
package_dict["A200S_V3"] = [['a200s_v3', default_name]]
|
package_dict["A200S_V3"] = [['a200s_v3', default_name]]
|
||||||
package_dict["A200S_V4"] = [['a200s_v4', default_name]]
|
package_dict["A200S_V4"] = [['a200s_v4', default_name]]
|
||||||
|
package_dict["A200S_V4.1"] = [['a200s_v41', default_name]]
|
||||||
package_dict["100_250"] = [['100_250', default_name],
|
package_dict["100_250"] = [['100_250', default_name],
|
||||||
['100_250_no_limits', no_limits_name]]
|
['100_250_no_limits', no_limits_name]]
|
||||||
package_dict["100_250_MKIII"] = [['100_250_mkiii', default_name],
|
package_dict["100_250_MKIII"] = [['100_250_mkiii', default_name],
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue