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

Merge pull request #2 from Mitchlol/merge_new_master 

=== FW 3.60 ===
This commit is contained in:
Mitch Lustig 2019-09-08 18:35:44 -07:00 committed by GitHub
parent 05d81a6e66 72e9c24bd7
commit 896f4876d5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
89 changed files with 11636 additions and 384 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
CHANGELOG
View File

@ -1,3 +1,29 @@
=== FW 3.60 ===
* Fixed IMU9x50 bug.
* Unrigester ICM20948 terminal callbacks when unused.
* Added experiment plot functions.
* Added D and Q axis voltage to RT data.
* Added smart reverse function to nunchuk app.
=== FW 3.59 ===
* Added more data to MOTE_PACKET_ALIVE.
* Added app template.
* Added function to unregister terminal callbacks.
* Added BMI160 support.
* Added support for the VESC HD.
* Added support for SWD programming permanent NRF.
* Encoder SW SPI fix.
* Slightly faster boot.
* Moved custom HW and APP configurations to conf_general.h.
* Added support for passing HW and APP default configuration as make arguments.
* Added SW shutdown support.
* Added command to erase bootloader.
* Added function to stop IMU threads, so that IMUs can be switched during runtime.
* Only generate encoder fault when the ERPM is low enough to use the encoder.
* Added many IMU and AHRS settings to appconf.
* Re-initialize IMU when appconf is written.
* Added imu_gyro_info terminal command.
=== FW 3.58 ===
* Set motor to FOC mode after successful FOC detection instead of the default type for the hardware.
* APP_ADC: Do not send brake command over CAN if config.multi_esc is not set.

1
Makefile
View File

@ -155,6 +155,7 @@ CSRC = $(STARTUPSRC) \
timer.c \
i2c_bb.c \
virtual_motor.c \
shutdown.c \
$(HWSRC) \
$(APPSRC) \
$(NRFSRC) \

130
appconf/appconf_default.h
View File

@ -51,6 +51,9 @@
#ifndef APPCONF_PERMANENT_UART_ENABLED
#define APPCONF_PERMANENT_UART_ENABLED true
#endif
#ifndef APPCONF_SHUTDOWN_MODE
#define APPCONF_SHUTDOWN_MODE SHUTDOWN_MODE_OFF_AFTER_30M
#endif
// The default app is UART in case the UART port is used for
// firmware updates.
@ -93,10 +96,10 @@
#define APPCONF_PPM_THROTTLE_EXP_MODE THR_EXP_POLY
#endif
#ifndef APPCONF_PPM_RAMP_TIME_POS
#define APPCONF_PPM_RAMP_TIME_POS 0.3
#define APPCONF_PPM_RAMP_TIME_POS 0.4
#endif
#ifndef APPCONF_PPM_RAMP_TIME_NEG
#define APPCONF_PPM_RAMP_TIME_NEG 0.1
#define APPCONF_PPM_RAMP_TIME_NEG 0.2
#endif
#ifndef APPCONF_PPM_MULTI_ESC
#define APPCONF_PPM_MULTI_ESC true
@ -192,10 +195,10 @@
#define APPCONF_CHUK_HYST 0.15
#endif
#ifndef APPCONF_CHUK_RAMP_TIME_POS
#define APPCONF_CHUK_RAMP_TIME_POS 0.9
#define APPCONF_CHUK_RAMP_TIME_POS 0.4
#endif
#ifndef APPCONF_CHUK_RAMP_TIME_NEG
#define APPCONF_CHUK_RAMP_TIME_NEG 0.3
#define APPCONF_CHUK_RAMP_TIME_NEG 0.2
#endif
#ifndef APPCONF_STICK_ERPM_PER_S_IN_CC
#define APPCONF_STICK_ERPM_PER_S_IN_CC 3000.0
@ -218,6 +221,15 @@
#ifndef APPCONF_CHUK_TC_MAX_DIFF
#define APPCONF_CHUK_TC_MAX_DIFF 3000.0
#endif
#ifndef APPCONF_CHUK_USE_SMART_REV
#define APPCONF_CHUK_USE_SMART_REV true
#endif
#ifndef APPCONF_CHUK_SMART_REV_MAX_DUTY
#define APPCONF_CHUK_SMART_REV_MAX_DUTY 0.07
#endif
#ifndef APPCONF_CHUK_SMART_REV_RAMP_TIME
#define APPCONF_CHUK_SMART_REV_RAMP_TIME 3.0
#endif
// NRF app
#ifndef APPCONF_NRF_SPEED
@ -261,38 +273,20 @@
#ifndef APPCONF_BALANCE_KD
#define APPCONF_BALANCE_KD 0.0
#endif
#ifndef APPCONF_BALANCE_LOOP_DELAY
#define APPCONF_BALANCE_LOOP_DELAY 5
#ifndef APPCONF_BALANCE_HERTZ
#define APPCONF_BALANCE_HERTZ 1000
#endif
#ifndef APPCONF_BALANCE_M_ACD
#define APPCONF_BALANCE_M_ACD 1.0
#ifndef APPCONF_BALANCE_M_AXIS
#define APPCONF_BALANCE_M_AXIS 0
#endif
#ifndef APPCONF_BALANCE_M_B
#define APPCONF_BALANCE_M_B 0.1
#ifndef APPCONF_BALANCE_C_AXIS
#define APPCONF_BALANCE_C_AXIS 1
#endif
#ifndef APPCONF_BALANCE_CAL_DELAY
#define APPCONF_BALANCE_CAL_DELAY 1500
#ifndef APPCONF_BALANCE_M_FAULT
#define APPCONF_BALANCE_M_FAULT 20
#endif
#ifndef APPCONF_BALANCE_CAL_M_ACD
#define APPCONF_BALANCE_CAL_M_ACD 1.0
#endif
#ifndef APPCONF_BALANCE_CAL_M_B
#define APPCONF_BALANCE_CAL_M_B 2.0
#endif
#ifndef APPCONF_BALANCE_PITCH_OFFSET
#define APPCONF_BALANCE_PITCH_OFFSET 0
#endif
#ifndef APPCONF_BALANCE_ROLL_OFFSET
#define APPCONF_BALANCE_ROLL_OFFSET 0
#endif
#ifndef APPCONF_BALANCE_USE_PERIPHERAL
#define APPCONF_BALANCE_USE_PERIPHERAL false
#endif
#ifndef APPCONF_BALANCE_PITCH_FAULT
#define APPCONF_BALANCE_PITCH_FAULT 45
#endif
#ifndef APPCONF_BALANCE_ROLL_FAULT
#define APPCONF_BALANCE_ROLL_FAULT 45
#ifndef APPCONF_BALANCE_C_FAULT
#define APPCONF_BALANCE_C_FAULT 45
#endif
#ifndef APPCONF_BALANCE_OVERSPEED_DUTY
#define APPCONF_BALANCE_OVERSPEED_DUTY 0.9
@ -306,14 +300,14 @@
#ifndef APPCONF_BALANCE_TILTBACK_SPEED
#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
#endif
#ifndef APPCONF_BALANCE_STARTUP_PITCH
#define APPCONF_BALANCE_STARTUP_PITCH 30.0
#ifndef APPCONF_BALANCE_STARTUP_M_TOLERANCE
#define APPCONF_BALANCE_STARTUP_M_TOLERANCE 20.0
#endif
#ifndef APPCONF_BALANCE_STARTUP_ROLL
#define APPCONF_BALANCE_STARTUP_ROLL 5.0
#ifndef APPCONF_BALANCE_STARTUP_C_TOLERANCE
#define APPCONF_BALANCE_STARTUP_C_TOLERANCE 8.0
#endif
#ifndef APPCONF_BALANCE_STARTUP_SPEED
#define APPCONF_BALANCE_STARTUP_SPEED 40.0
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
#endif
#ifndef APPCONF_BALANCE_DEADZONE
#define APPCONF_BALANCE_DEADZONE 0.0
@ -322,4 +316,66 @@
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
#endif
// IMU
#ifndef APPCONF_IMU_TYPE
#define APPCONF_IMU_TYPE IMU_TYPE_INTERNAL
#endif
#ifndef APPCONF_IMU_AHRS_MODE
#define APPCONF_IMU_AHRS_MODE AHRS_MODE_MADGWICK
#endif
#ifndef APPCONF_IMU_SAMPLE_RATE_HZ
#define APPCONF_IMU_SAMPLE_RATE_HZ 200
#endif
#ifndef APPCONF_IMU_ACCEL_CONFIDENCE_DECAY
#define APPCONF_IMU_ACCEL_CONFIDENCE_DECAY 1.0
#endif
#ifndef APPCONF_IMU_MAHONY_KP
#define APPCONF_IMU_MAHONY_KP 0.3
#endif
#ifndef APPCONF_IMU_MAHONY_KI
#define APPCONF_IMU_MAHONY_KI 0.0
#endif
#ifndef APPCONF_IMU_MADGWICK_BETA
#define APPCONF_IMU_MADGWICK_BETA 0.1
#endif
#ifndef APPCONF_IMU_ROT_ROLL
#define APPCONF_IMU_ROT_ROLL 0.0
#endif
#ifndef APPCONF_IMU_ROT_PITCH
#define APPCONF_IMU_ROT_PITCH 0.0
#endif
#ifndef APPCONF_IMU_ROT_YAW
#define APPCONF_IMU_ROT_YAW 0.0
#endif
#ifndef APPCONF_IMU_A_OFFSET_0
#define APPCONF_IMU_A_OFFSET_0 0.0
#endif
#ifndef APPCONF_IMU_A_OFFSET_1
#define APPCONF_IMU_A_OFFSET_1 0.0
#endif
#ifndef APPCONF_IMU_A_OFFSET_2
#define APPCONF_IMU_A_OFFSET_2 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_0
#define APPCONF_IMU_G_OFFSET_0 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_1
#define APPCONF_IMU_G_OFFSET_1 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_2
#define APPCONF_IMU_G_OFFSET_2 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_0
#define APPCONF_IMU_G_OFFSET_COMP_FACT_0 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_1
#define APPCONF_IMU_G_OFFSET_COMP_FACT_1 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_2
#define APPCONF_IMU_G_OFFSET_COMP_FACT_2 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_CLAMP
#define APPCONF_IMU_G_OFFSET_COMP_CLAMP 5.0
#endif
#endif /* APPCONF_DEFAULT_H_ */

3
applications/app.c
View File

@ -24,6 +24,7 @@
#include "nrf_driver.h"
#include "rfhelp.h"
#include "comm_can.h"
#include "imu.h"
// Private variables
static app_configuration appconf;
@ -65,6 +66,8 @@ void app_set_configuration(app_configuration *conf) {
app_custom_stop();
#endif
imu_init(&conf->imu_conf);
// Configure balance app before starting it.
app_balance_configure(&appconf.app_balance_conf);

1
applications/app.h
View File

@ -69,6 +69,5 @@ uint16_t app_balance_get_state(void);
void app_custom_start(void);
void app_custom_stop(void);
void app_custom_configure(app_configuration *conf);
void app_custom_terminal_rpy(int argc, const char **argv);
#endif /* APP_H_ */

3
applications/app_adc.c
View File

@ -233,7 +233,8 @@ static THD_FUNCTION(adc_thread, arg) {
if (config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON ||
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER ||
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON ||
config.ctrl_type == ADC_CTRL_TYPE_DUTY_REV_BUTTON) {
config.ctrl_type == ADC_CTRL_TYPE_DUTY_REV_BUTTON ||
config.ctrl_type == ADC_CTRL_TYPE_PID_REV_BUTTON) {
rev_button = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
if (config.rev_button_inverted) {
rev_button = !rev_button;

138
applications/app_balance.c
View File

@ -28,20 +28,21 @@
#include "imu/imu.h"
#include "imu/ahrs.h"
#include "utils.h"
#include "datatypes.h"
#include <math.h>
// Data type
typedef enum {
CALIBRATING = 0,
STARTUP = 0,
RUNNING,
FAULT,
DEAD
} BalanceState;
typedef enum {
STARTUP = 0,
CENTERING = 0,
TILTBACK
} SetpointAdjustmentType;
@ -49,12 +50,12 @@ typedef enum {
static THD_FUNCTION(balance_thread, arg);
static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
static volatile balance_config config;
static volatile balance_config balance_conf;
static thread_t *app_thread;
// Values used in loop
static BalanceState state;
static float pitch, roll;
static float m_angle, c_angle;
static float proportional, integral, derivative;
static float last_proportional;
static float pid_value;
@ -62,27 +63,22 @@ static float setpoint, setpoint_target;
static SetpointAdjustmentType setpointAdjustmentType;
static float startup_step_size, tiltback_step_size;
static systime_t current_time, last_time, diff_time;
static systime_t cal_start_time, cal_diff_time;
static systime_t startup_start_time, startup_diff_time;
// Values read to pass in app data to GUI
static float motor_current;
static float motor_position;
void app_balance_configure(balance_config *conf) {
config = *conf;
balance_conf = *conf;
}
void app_balance_start(void) {
// Reset IMU
if(config.use_peripheral){
imu_init(true);
}
// Reset all Values
state = CALIBRATING;
pitch = 0;
roll = 0;
state = STARTUP;
m_angle = 0;
c_angle = 0;
proportional = 0;
integral = 0;
derivative = 0;
@ -90,14 +86,14 @@ void app_balance_start(void) {
pid_value = 0;
setpoint = 0;
setpoint_target = 0;
setpointAdjustmentType = STARTUP;
setpointAdjustmentType = CENTERING;
startup_step_size = 0;
tiltback_step_size = 0;
current_time = 0;
last_time = 0;
diff_time = 0;
cal_start_time = 0;
cal_diff_time = 0;
startup_start_time = 0;
startup_diff_time = 0;
// Start the balance thread
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
@ -109,21 +105,16 @@ void app_balance_stop(void) {
chThdWait(app_thread);
}
mc_interface_set_current(0);
// Reset IMU
if(config.use_peripheral){
imu_init(false);
}
}
float app_balance_get_pid_output(void) {
return pid_value;
}
float app_balance_get_pitch(void) {
return pitch;
return m_angle;
}
float app_balance_get_roll(void) {
return roll;
return c_angle;
}
uint32_t app_balance_get_diff_time(void) {
return ST2US(diff_time);
@ -140,7 +131,7 @@ uint16_t app_balance_get_state(void) {
float get_setpoint_adjustment_step_size(void){
switch(setpointAdjustmentType){
case (STARTUP):
case (CENTERING):
return startup_step_size;
case (TILTBACK):
return tiltback_step_size;
@ -149,16 +140,16 @@ float get_setpoint_adjustment_step_size(void){
}
float apply_deadzone(float error){
if(config.deadzone == 0){
if(balance_conf.deadzone == 0){
return error;
}
if(error < config.deadzone && error > -config.deadzone){
if(error < balance_conf.deadzone && error > -balance_conf.deadzone){
return 0;
} else if(error > config.deadzone){
return error - config.deadzone;
} else if(error > balance_conf.deadzone){
return error - balance_conf.deadzone;
} else {
return error + config.deadzone;
return error + balance_conf.deadzone;
}
}
@ -167,11 +158,11 @@ static THD_FUNCTION(balance_thread, arg) {
chRegSetThreadName("APP_BALANCE");
// Do one off config
startup_step_size = (config.startup_speed / 1000) * config.loop_delay;
tiltback_step_size = (config.tiltback_speed / 1000) * config.loop_delay;
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
state = CALIBRATING;
setpointAdjustmentType = STARTUP;
state = STARTUP;
setpointAdjustmentType = CENTERING;
while (!chThdShouldTerminateX()) {
// Update times
@ -186,54 +177,63 @@ static THD_FUNCTION(balance_thread, arg) {
motor_current = mc_interface_get_tot_current_directional_filtered();
motor_position = mc_interface_get_pid_pos_now();
// Read gyro values
pitch = imu_get_pitch() * 180.0f / M_PI;
roll = imu_get_roll() * 180.0f / M_PI;
// Apply offsets
pitch = fmodf(((pitch + 180.0f) + config.pitch_offset), 360.0f) - 180.0f;
roll = fmodf(((roll + 180.0f) + config.roll_offset), 360.0f) - 180.0f;
// Get the values we want
switch(balance_conf.m_axis){
case (PITCH):
m_angle = imu_get_pitch() * 180.0f / M_PI;;
break;
case (ROLL):
m_angle = imu_get_roll() * 180.0f / M_PI;
break;
case (YAW):
m_angle = imu_get_yaw() * 180.0f / M_PI;
break;
}
switch(balance_conf.c_axis){
case (PITCH):
c_angle = imu_get_pitch() * 180.0f / M_PI;;
break;
case (ROLL):
c_angle = imu_get_roll() * 180.0f / M_PI;
break;
case (YAW):
c_angle = imu_get_yaw() * 180.0f / M_PI;
break;
}
// State based logic
switch(state){
case (CALIBRATING):
if(cal_start_time == 0){
cal_start_time = current_time;
ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd);
ahrs_set_madgwick_beta(config.cal_m_b);
case (STARTUP):
if(startup_start_time == 0){
startup_start_time = current_time;
}
cal_diff_time = current_time - cal_start_time;
startup_diff_time = current_time - startup_start_time;
// Calibration is done
if(ST2MS(cal_diff_time) > config.cal_delay){
// Set gyro config to be running config
ahrs_set_madgwick_acc_confidence_decay(config.m_acd);
ahrs_set_madgwick_beta(config.m_b);
if(ST2MS(startup_diff_time) > 1000){
// Set fault and wait for valid startup condition
state = FAULT;
cal_start_time = 0;
cal_diff_time = 0;
startup_start_time = 0;
startup_diff_time = 0;
}
break;
case (RUNNING):
// Check for overspeed
if(mc_interface_get_duty_cycle_now() > config.overspeed_duty || mc_interface_get_duty_cycle_now() < -config.overspeed_duty){
if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){
state = DEAD;
}
// Check for fault
if(pitch > config.pitch_fault || pitch < -config.pitch_fault || roll > config.roll_fault || roll < -config.roll_fault){
if(fabsf(m_angle) > balance_conf.m_fault || fabsf(c_angle) > balance_conf.c_fault){
state = FAULT;
}
// Over speed tilt back safety
if(mc_interface_get_duty_cycle_now() > config.tiltback_duty){
setpoint_target = config.tiltback_angle;
if(mc_interface_get_duty_cycle_now() > balance_conf.tiltback_duty){
setpoint_target = balance_conf.tiltback_angle;
setpointAdjustmentType = TILTBACK;
} else if(mc_interface_get_duty_cycle_now() < -config.tiltback_duty){
setpoint_target = -config.tiltback_angle;
} else if(mc_interface_get_duty_cycle_now() < -balance_conf.tiltback_duty){
setpoint_target = -balance_conf.tiltback_angle;
setpointAdjustmentType = TILTBACK;
}else{
setpoint_target = 0;
@ -252,22 +252,22 @@ static THD_FUNCTION(balance_thread, arg) {
}
// Do PID maths
proportional = setpoint - pitch;
proportional = setpoint - m_angle;
// Apply deadzone
proportional = apply_deadzone(proportional);
// Resume real PID maths
integral = integral + proportional;
derivative = proportional - last_proportional;
pid_value = (config.kp * proportional) + (config.ki * integral) + (config.kd * derivative);
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
last_proportional = proportional;
// Apply current boost
if(pid_value > 0){
pid_value += config.current_boost;
pid_value += balance_conf.current_boost;
}else if(pid_value < 0){
pid_value -= config.current_boost;
pid_value -= balance_conf.current_boost;
}
// Reset the timeout
@ -282,10 +282,10 @@ static THD_FUNCTION(balance_thread, arg) {
break;
case (FAULT):
// Check for valid startup position
if(pitch < config.startup_pitch && pitch > -config.startup_pitch && roll < config.startup_roll && roll > -config.startup_roll){
setpoint = pitch;
if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance){
setpoint = m_angle;
setpoint_target = 0;
setpointAdjustmentType = STARTUP;
setpointAdjustmentType = CENTERING;
state = RUNNING;
break;
}
@ -300,7 +300,7 @@ static THD_FUNCTION(balance_thread, arg) {
}
// Delay between loops
chThdSleepMilliseconds(config.loop_delay);
chThdSleepMicroseconds((int)((1000.0 / balance_conf.hertz) * 1000.0));
}
// Disable output

136
applications/app_custom_template.c Normal file
View File

@ -0,0 +1,136 @@
/*
Copyright 2019 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/>.
*/
#include "app.h"
#include "ch.h"
#include "hal.h"
// Some useful includes
#include "mc_interface.h"
#include "utils.h"
#include "encoder.h"
#include "terminal.h"
#include "comm_can.h"
#include "hw.h"
#include "commands.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
// Threads
static THD_FUNCTION(my_thread, arg);
static THD_WORKING_AREA(my_thread_wa, 2048);
// Private functions
static void pwm_callback(void);
static void terminal_test(int argc, const char **argv);
// Private variables
static volatile bool stop_now = true;
static volatile bool is_running = false;
// Called when the custom application is started. Start our
// threads here and set up callbacks.
void app_custom_start(void) {
mc_interface_set_pwm_callback(pwm_callback);
stop_now = false;
chThdCreateStatic(my_thread_wa, sizeof(my_thread_wa),
NORMALPRIO, my_thread, NULL);
// Terminal commands for the VESC Tool terminal can be registered.
terminal_register_command_callback(
"custom_cmd",
"Print the number d",
"[d]",
terminal_test);
}
// Called when the custom application is stopped. Stop our threads
// and release callbacks.
void app_custom_stop(void) {
mc_interface_set_pwm_callback(0);
terminal_unregister_callback(terminal_test);
stop_now = true;
while (is_running) {
chThdSleepMilliseconds(1);
}
}
void app_custom_configure(app_configuration *conf) {
(void)conf;
}
static THD_FUNCTION(my_thread, arg) {
(void)arg;
chRegSetThreadName("App Custom");
is_running = true;
// Example of using the experiment plot
// chThdSleepMilliseconds(8000);
// commands_init_plot("Sample", "Voltage");
// commands_plot_add_graph("Temp Fet");
// commands_plot_add_graph("Input Voltage");
// float samp = 0.0;
//
// for(;;) {
// commands_plot_set_graph(0);
// commands_send_plot_points(samp, mc_interface_temp_fet_filtered());
// commands_plot_set_graph(1);
// commands_send_plot_points(samp, GET_INPUT_VOLTAGE());
// samp++;
// chThdSleepMilliseconds(10);
// }
for(;;) {
// Check if it is time to stop.
if (stop_now) {
is_running = false;
return;
}
// Run your logic here. A lot of functionality is available in mc_interface.h.
chThdSleepMicroseconds(10);
}
}
static void pwm_callback(void) {
// Called for every control iteration in interrupt context.
}
// Callback function for the terminal command with arguments.
static void terminal_test(int argc, const char **argv) {
if (argc == 2) {
int d = -1;
sscanf(argv[1], "%d", &d);
commands_printf("You have entered %d", d);
// For example, read the ADC inputs on the COMM header.
commands_printf("ADC1: %.2f V ADC2: %.2f V",
(double)ADC_VOLTS(ADC_IND_EXT), (double)ADC_VOLTS(ADC_IND_EXT2));
} else {
commands_printf("This command requires one argument.\n");
}
}

49
applications/app_nunchuk.c
View File

@ -213,6 +213,8 @@ static THD_FUNCTION(output_thread, arg) {
static float rpm_filtered = 0.0;
UTILS_LP_FAST(rpm_filtered, mc_interface_get_rpm(), 0.5);
const float dt = (float)OUTPUT_ITERATION_TIME_MS / 1000.0;
if (timeout_has_timeout() || chuck_error != 0 || config.ctrl_type == CHUK_CTRL_TYPE_NONE) {
was_pid = false;
continue;
@ -234,6 +236,7 @@ static THD_FUNCTION(output_thread, arg) {
static bool is_reverse = false;
static bool was_z = false;
const float current_now = mc_interface_get_tot_current_directional_filtered();
const float duty_now = mc_interface_get_duty_cycle_now();
static float prev_current = 0.0;
const float max_current_diff = mcconf->l_current_max * mcconf->l_current_max_scale * 0.2;
@ -367,6 +370,7 @@ static THD_FUNCTION(output_thread, arg) {
float rpm_lowest = rpm_local;
float current_highest_abs = current_now;
float duty_highest_abs = fabsf(duty_now);
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
@ -391,10 +395,55 @@ static THD_FUNCTION(output_thread, arg) {
if (fabsf(msg_current) > fabsf(current_highest_abs)) {
current_highest_abs = msg_current;
}
if (fabsf(msg->duty) > duty_highest_abs) {
duty_highest_abs = fabsf(msg->duty);
}
}
}
}
if (config.use_smart_rev) {
bool duty_control = false;
static bool was_duty_control = false;
static float duty_rev = 0.0;
if (out_val < -0.95 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
current_highest_abs < (mcconf->l_current_max * mcconf->l_current_max_scale * 0.7)) {
duty_control = true;
}
if (duty_control || (was_duty_control && out_val < -0.1)) {
was_duty_control = true;
float goal = config.smart_rev_max_duty * -out_val;
utils_step_towards(&duty_rev, is_reverse ? goal : -goal,
config.smart_rev_max_duty * dt / config.smart_rev_ramp_time);
mc_interface_set_duty(duty_rev);
// Send the same duty cycle to the other controllers
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_duty(msg->id, duty_rev);
}
}
}
// Set the previous ramping current to not get a spike when releasing
// duty control.
prev_current = current_now;
continue;
}
duty_rev = duty_now;
was_duty_control = false;
}
// Apply ramping
const float current_range = mcconf->l_current_max * mcconf->l_current_max_scale +
fabsf(mcconf->l_current_min) * mcconf->l_current_min_scale;

8
blackmagic/bm_if.c
View File

@ -513,8 +513,8 @@ void bm_change_swd_pins(stm32_gpio_t *swdio_port, int swdio_pin,
bm_set_enabled(false);
platform_swdio_port = swdio_port;
platform_swdio_pin = swdio_pin;
platform_swdio_port = swclk_port;
platform_swdio_pin = swclk_pin;
platform_swclk_port = swclk_port;
platform_swclk_pin = swclk_pin;
}
/**
@ -524,6 +524,6 @@ void bm_default_swd_pins(void) {
bm_set_enabled(false);
platform_swdio_port = SWDIO_PORT_DEFAULT;
platform_swdio_pin = SWDIO_PIN_DEFAULT;
platform_swdio_port = SWCLK_PORT_DEFAULT;
platform_swdio_pin = SWCLK_PIN_DEFAULT;
platform_swclk_port = SWCLK_PORT_DEFAULT;
platform_swclk_pin = SWCLK_PIN_DEFAULT;
}

BIN
build_all/410_o_411_o_412/VESC_0005ohm.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_0005ohm.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_33k.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_ws2811_33k.bin
View File

Binary file not shown.

BIN
build_all/48/VESC_0005ohm.bin
View File

Binary file not shown.

BIN
build_all/48/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/48/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/48/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_servoout.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_ws2811.bin
View File

Binary file not shown.

BIN
build_all/AXIOM/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/DAS_RS/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/HD/VESC_default.bin Executable file
View File

Binary file not shown.

BIN
build_all/HD/VESC_default_no_hw_limits.bin Executable file
View File

Binary file not shown.

BIN
build_all/HD/VESC_servoout.bin Executable file
View File

Binary file not shown.

BIN
build_all/HD/VESC_ws2811.bin Executable file
View File

Binary file not shown.

BIN
build_all/UAVC_OMEGA/VESC_default.bin Executable file
View File

Binary file not shown.

55
build_all/rebuild_all
View File

@ -272,3 +272,58 @@ cd $FWPATH
make clean
cd $DIR
#################### HW UAVC_OMEGA ########################
COPYDIR=UAVC_OMEGA
rm -f $COPYDIR/*
# default
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DHW_SOURCE=\"hw_uavc_omega.c\" -DHW_HEADER=\"hw_uavc_omega.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
# Clean
cd $FWPATH
make clean
cd $DIR
#################### HW HD ########################
COPYDIR=HD
rm -f $COPYDIR/*
# default
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DHW_SOURCE=\"hw_hd.c\" -DHW_HEADER=\"hw_hd.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
# default with HW limits disables
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_hd.c\" -DHW_HEADER=\"hw_hd.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
# ws2811
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_hd.c\" -DHW_HEADER=\"hw_hd.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
# servoout
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_hd.c\" -DHW_HEADER=\"hw_hd.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
# Clean
cd $FWPATH
make clean
cd $DIR

157
commands.c
View File

@ -41,6 +41,7 @@
#include "gpdrive.h"
#include "confgenerator.h"
#include "imu.h"
#include "shutdown.h"
#if HAS_BLACKMAGIC
#include "bm_if.h"
#endif
@ -319,6 +320,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
buffer_append_float16(send_buffer, NTC_TEMP_MOS2(), 1e1, &ind);
buffer_append_float16(send_buffer, NTC_TEMP_MOS3(), 1e1, &ind);
}
if (mask & ((uint32_t)1 << 19)) {
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vd(), 1e3, &ind);
}
if (mask & ((uint32_t)1 << 20)) {
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vq(), 1e3, &ind);
}
reply_func(send_buffer, ind);
chMtxUnlock(&send_buffer_mutex);
@ -471,6 +478,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
break;
case COMM_ALIVE:
SHUTDOWN_RESET();
timeout_reset();
break;
@ -616,45 +624,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
case COMM_GET_VALUES_SETUP:
case COMM_GET_VALUES_SETUP_SELECTIVE: {
float ah_tot = 0.0;
float ah_charge_tot = 0.0;
float wh_tot = 0.0;
float wh_charge_tot = 0.0;
float current_tot = 0.0;
float current_in_tot = 0.0;
uint8_t num_vescs = 1;
ah_tot += mc_interface_get_amp_hours(false);
ah_charge_tot += mc_interface_get_amp_hours_charged(false);
wh_tot += mc_interface_get_watt_hours(false);
wh_charge_tot += mc_interface_get_watt_hours_charged(false);
current_tot += mc_interface_get_tot_current_filtered();
current_in_tot += mc_interface_get_tot_current_in_filtered();
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
current_tot += msg->current;
num_vescs++;
}
can_status_msg_2 *msg2 = comm_can_get_status_msg_2_index(i);
if (msg2->id >= 0 && UTILS_AGE_S(msg2->rx_time) < 0.1) {
ah_tot += msg2->amp_hours;
ah_charge_tot += msg2->amp_hours_charged;
}
can_status_msg_3 *msg3 = comm_can_get_status_msg_3_index(i);
if (msg3->id >= 0 && UTILS_AGE_S(msg3->rx_time) < 0.1) {
wh_tot += msg3->watt_hours;
wh_charge_tot += msg3->watt_hours_charged;
}
can_status_msg_4 *msg4 = comm_can_get_status_msg_4_index(i);
if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 0.1) {
current_in_tot += msg4->current_in;
}
}
setup_values val = mc_interface_get_setup_values();
float wh_batt_left = 0.0;
float battery_level = mc_interface_get_battery_level(&wh_batt_left);
@ -678,10 +648,10 @@ void commands_process_packet(unsigned char *data, unsigned int len,
buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
}
if (mask & ((uint32_t)1 << 2)) {
buffer_append_float32(send_buffer, current_tot, 1e2, &ind);
buffer_append_float32(send_buffer, val.current_tot, 1e2, &ind);
}
if (mask & ((uint32_t)1 << 3)) {
buffer_append_float32(send_buffer, current_in_tot, 1e2, &ind);
buffer_append_float32(send_buffer, val.current_in_tot, 1e2, &ind);
}
if (mask & ((uint32_t)1 << 4)) {
buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
@ -699,16 +669,16 @@ void commands_process_packet(unsigned char *data, unsigned int len,
buffer_append_float16(send_buffer, battery_level, 1e3, &ind);
}
if (mask & ((uint32_t)1 << 9)) {
buffer_append_float32(send_buffer, ah_tot, 1e4, &ind);
buffer_append_float32(send_buffer, val.ah_tot, 1e4, &ind);
}
if (mask & ((uint32_t)1 << 10)) {
buffer_append_float32(send_buffer, ah_charge_tot, 1e4, &ind);
buffer_append_float32(send_buffer, val.ah_charge_tot, 1e4, &ind);
}
if (mask & ((uint32_t)1 << 11)) {
buffer_append_float32(send_buffer, wh_tot, 1e4, &ind);
buffer_append_float32(send_buffer, val.wh_tot, 1e4, &ind);
}
if (mask & ((uint32_t)1 << 12)) {
buffer_append_float32(send_buffer, wh_charge_tot, 1e4, &ind);
buffer_append_float32(send_buffer, val.wh_charge_tot, 1e4, &ind);
}
if (mask & ((uint32_t)1 << 13)) {
buffer_append_float32(send_buffer, mc_interface_get_distance(), 1e3, &ind);
@ -726,7 +696,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
send_buffer[ind++] = app_get_configuration()->controller_id;
}
if (mask & ((uint32_t)1 << 18)) {
send_buffer[ind++] = num_vescs;
send_buffer[ind++] = val.num_vescs;
}
if (mask & ((uint32_t)1 << 19)) {
buffer_append_float32(send_buffer, wh_batt_left, 1e3, &ind);
@ -938,6 +908,31 @@ void commands_process_packet(unsigned char *data, unsigned int len,
reply_func(send_buffer, ind);
} break;
case COMM_ERASE_BOOTLOADER_ALL_CAN:
if (nrf_driver_ext_nrf_running()) {
nrf_driver_pause(6000);
}
data[-1] = COMM_ERASE_BOOTLOADER;
comm_can_send_buffer(255, data - 1, len + 1, 2);
chThdSleepMilliseconds(1500);
/* Falls through. */
/* no break */
case COMM_ERASE_BOOTLOADER: {
int32_t ind = 0;
if (nrf_driver_ext_nrf_running()) {
nrf_driver_pause(6000);
}
uint16_t flash_res = flash_helper_erase_bootloader();
ind = 0;
uint8_t send_buffer[50];
send_buffer[ind++] = COMM_ERASE_BOOTLOADER;
send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
reply_func(send_buffer, ind);
} break;
// Blocking commands. Only one of them runs at any given time, in their
// own thread. If other blocking commands come before the previous one has
// finished, they are discarded.
@ -955,6 +950,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
case COMM_BM_WRITE_FLASH:
case COMM_BM_REBOOT:
case COMM_BM_DISCONNECT:
case COMM_BM_MAP_PINS_DEFAULT:
case COMM_BM_MAP_PINS_NRF5X:
if (!is_blocking) {
memcpy(blocking_thread_cmd_buffer, data - 1, len + 1);
blocking_thread_cmd_len = len;
@ -1106,6 +1103,48 @@ void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
#endif
}
void commands_init_plot(char *namex, char *namey) {
int ind = 0;
chMtxLock(&send_buffer_mutex);
send_buffer_global[ind++] = COMM_PLOT_INIT;
memcpy(send_buffer_global + ind, namex, strlen(namex));
ind += strlen(namex);
send_buffer_global[ind++] = '\0';
memcpy(send_buffer_global + ind, namey, strlen(namey));
ind += strlen(namey);
send_buffer_global[ind++] = '\0';
commands_send_packet(send_buffer_global, ind);
chMtxUnlock(&send_buffer_mutex);
}
void commands_plot_add_graph(char *name) {
int ind = 0;
chMtxLock(&send_buffer_mutex);
send_buffer_global[ind++] = COMM_PLOT_ADD_GRAPH;
memcpy(send_buffer_global + ind, name, strlen(name));
ind += strlen(name);
send_buffer_global[ind++] = '\0';
commands_send_packet(send_buffer_global, ind);
chMtxUnlock(&send_buffer_mutex);
}
void commands_plot_set_graph(int graph) {
int ind = 0;
uint8_t buffer[2];
buffer[ind++] = COMM_PLOT_SET_GRAPH;
buffer[ind++] = graph;
commands_send_packet(buffer, ind);
}
void commands_send_plot_points(float x, float y) {
int32_t ind = 0;
uint8_t buffer[10];
buffer[ind++] = COMM_PLOT_DATA;
buffer_append_float32_auto(buffer, x, &ind);
buffer_append_float32_auto(buffer, y, &ind);
commands_send_packet(buffer, ind);
}
static THD_FUNCTION(blocking_thread, arg) {
(void)arg;
@ -1401,6 +1440,32 @@ static THD_FUNCTION(blocking_thread, arg) {
send_func_blocking(send_buffer, ind);
}
} break;
case COMM_BM_MAP_PINS_DEFAULT: {
bm_default_swd_pins();
int32_t ind = 0;
send_buffer[ind++] = packet_id;
buffer_append_int16(send_buffer, 1, &ind);
if (send_func_blocking) {
send_func_blocking(send_buffer, ind);
}
} break;
case COMM_BM_MAP_PINS_NRF5X: {
int32_t ind = 0;
send_buffer[ind++] = packet_id;
#ifdef NRF5x_SWDIO_GPIO
buffer_append_int16(send_buffer, 1, &ind);
bm_change_swd_pins(NRF5x_SWDIO_GPIO, NRF5x_SWDIO_PIN,
NRF5x_SWCLK_GPIO, NRF5x_SWCLK_PIN);
#else
buffer_append_int16(send_buffer, 0, &ind);
#endif
if (send_func_blocking) {
send_func_blocking(send_buffer, ind);
}
} break;
#endif
default:

4
commands.h
View File

@ -39,5 +39,9 @@ void commands_send_gpd_buffer_notify(void);
void commands_send_mcconf(COMM_PACKET_ID packet_id, mc_configuration *mcconf);
void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf);
void commands_apply_mcconf_hw_limits(mc_configuration *mcconf);
void commands_init_plot(char *namex, char *namey);
void commands_plot_add_graph(char *name);
void commands_plot_set_graph(int graph);
void commands_send_plot_points(float x, float y);
#endif /* COMMANDS_H_ */

2
conf_general.c
View File

@ -35,8 +35,6 @@
#include <string.h>
#include <math.h>
#include "conf_mc_app_default.h"
// EEPROM settings
#define EEPROM_BASE_MCCONF 1000
#define EEPROM_BASE_APPCONF 2000

40
conf_general.h
View File

@ -22,7 +22,7 @@
// Firmware version
#define FW_VERSION_MAJOR 3
#define FW_VERSION_MINOR 58
#define FW_VERSION_MINOR 60
#include "datatypes.h"
@ -106,11 +106,14 @@
//#define HW_SOURCE "hw_uavc_qcube.c"
//#define HW_HEADER "hw_uavc_qcube.h"
//#define HW_SOURCE "hw_uavc_basic.c"
//#define HW_HEADER "hw_uavc_basic.h"
//#define HW_SOURCE "hw_uavc_omega.c"
//#define HW_HEADER "hw_uavc_omega.h"
//#define HW_SOURCE "hw_binar_v1.c"
//#define HW_HEADER "hw_binar_v1.h"
//#define HW_SOURCE "hw_hd.c"
//#define HW_HEADER "hw_hd.h"
#endif
#ifndef HW_SOURCE
@ -121,29 +124,38 @@
#error "No hardware header file set"
#endif
#include "hw.h"
#ifdef USER_MC_CONF
#include USER_MC_CONF
#endif
#ifdef USER_APP_CONF
#include USER_APP_CONF
#endif
/*
* Select default user motor configuration
*/
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
//#define MCCONF_DEFAULT_USER "mcconf_sp_540kv.h"
//#define MCCONF_DEFAULT_USER "mcconf_castle_2028.h"
//#define MCCONF_DEFAULT_USER "mcconf_ellwee.h"
//#include "mcconf_sten.h"
//#include "mcconf_sp_540kv.h"
//#include "mcconf_castle_2028.h"
//#include "mcconf_ellwee.h"
//#include "conf_test.h"
/*
* Select default user app configuration
*/
//#define APPCONF_DEFAULT_USER "appconf_example_ppm.h"
//#define APPCONF_DEFAULT_USER "appconf_custom.h"
//#define APPCONF_DEFAULT_USER "appconf_ellwee.h"
//#include "appconf_example_ppm.h"
//#include "appconf_custom.h"
//#include "appconf_ellwee.h"
#include "hw.h"
#include "mcconf_default.h"
#include "appconf_default.h"
/*
* Set APP_CUSTOM_TO_USE to the name of the main C file of the custom application.
*/
//#define APP_CUSTOM_TO_USE "app_rotary_led.c"
//#define APPCONF_APP_TO_USE APP_CUSTOM
//#define MCCONF_FOC_F_SW 5000
//#define APP_CUSTOM_TO_USE "app_custom_template.c"
/*
* Enable blackmagic probe output on SWD port

134
confgenerator.c
View File

@ -1,7 +1,7 @@
// This file is autogenerated by VESC Tool
#include "buffer.h"
#include "conf_mc_app_default.h"
#include "conf_general.h"
#include "confgenerator.h"
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf) {
@ -153,6 +153,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer[ind++] = conf->can_baud_rate;
buffer[ind++] = conf->pairing_done;
buffer[ind++] = conf->permanent_uart_enabled;
buffer[ind++] = conf->shutdown_mode;
buffer[ind++] = conf->uavcan_enable;
buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
buffer[ind++] = conf->app_to_use;
@ -207,6 +208,9 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer[ind++] = conf->app_chuk_conf.multi_esc;
buffer[ind++] = conf->app_chuk_conf.tc;
buffer_append_float32_auto(buffer, conf->app_chuk_conf.tc_max_diff, &ind);
buffer[ind++] = conf->app_chuk_conf.use_smart_rev;
buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_max_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_ramp_time, &ind);
buffer[ind++] = conf->app_nrf_conf.speed;
buffer[ind++] = conf->app_nrf_conf.power;
buffer[ind++] = conf->app_nrf_conf.crc_type;
@ -220,26 +224,40 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.kp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.ki, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.kd, &ind);
buffer[ind++] = (uint8_t)conf->app_balance_conf.loop_delay;
buffer_append_float32_auto(buffer, conf->app_balance_conf.m_acd, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.m_b, &ind);
buffer_append_uint32(buffer, conf->app_balance_conf.cal_delay, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_acd, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_b, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_offset, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind);
buffer[ind++] = conf->app_balance_conf.use_peripheral;
buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind);
buffer[ind++] = conf->app_balance_conf.m_axis;
buffer[ind++] = conf->app_balance_conf.c_axis;
buffer_append_float32_auto(buffer, conf->app_balance_conf.m_fault, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.c_fault, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_angle, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_pitch, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_roll, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_m_tolerance, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_c_tolerance, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.deadzone, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.current_boost, &ind);
buffer[ind++] = conf->imu_conf.type;
buffer[ind++] = conf->imu_conf.mode;
buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.mahony_kp, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.mahony_ki, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.madgwick_beta, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.rot_roll, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.rot_pitch, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.rot_yaw, &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[0], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[1], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[2], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[0], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[1], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[2], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[0], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[1], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[2], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_clamp, &ind);
return ind;
}
@ -399,6 +417,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->can_baud_rate = buffer[ind++];
conf->pairing_done = buffer[ind++];
conf->permanent_uart_enabled = buffer[ind++];
conf->shutdown_mode = buffer[ind++];
conf->uavcan_enable = buffer[ind++];
conf->uavcan_esc_index = buffer[ind++];
conf->app_to_use = buffer[ind++];
@ -453,6 +472,9 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_chuk_conf.multi_esc = buffer[ind++];
conf->app_chuk_conf.tc = buffer[ind++];
conf->app_chuk_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind);
conf->app_chuk_conf.use_smart_rev = buffer[ind++];
conf->app_chuk_conf.smart_rev_max_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_chuk_conf.smart_rev_ramp_time = buffer_get_float32_auto(buffer, &ind);
conf->app_nrf_conf.speed = buffer[ind++];
conf->app_nrf_conf.power = buffer[ind++];
conf->app_nrf_conf.crc_type = buffer[ind++];
@ -466,26 +488,40 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.kp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.ki = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.kd = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.loop_delay = buffer[ind++];
conf->app_balance_conf.m_acd = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.m_b = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.cal_delay = buffer_get_uint32(buffer, &ind);
conf->app_balance_conf.cal_m_acd = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.pitch_offset = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.use_peripheral = buffer[ind++];
conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.m_axis = buffer[ind++];
conf->app_balance_conf.c_axis = buffer[ind++];
conf->app_balance_conf.m_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.c_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_angle = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_pitch = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_roll = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_m_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_c_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.deadzone = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.current_boost = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.type = buffer[ind++];
conf->imu_conf.mode = buffer[ind++];
conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind);
conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.mahony_kp = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.mahony_ki = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.madgwick_beta = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.rot_roll = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.rot_pitch = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.rot_yaw = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.accel_offsets[0] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.accel_offsets[1] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.accel_offsets[2] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offsets[0] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offsets[1] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offsets[2] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[0] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[1] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[2] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_clamp = buffer_get_float32_auto(buffer, &ind);
return true;
}
@ -629,6 +665,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->can_baud_rate = APPCONF_CAN_BAUD_RATE;
conf->pairing_done = APPCONF_PAIRING_DONE;
conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED;
conf->shutdown_mode = APPCONF_SHUTDOWN_MODE;
conf->uavcan_enable = APPCONF_UAVCAN_ENABLE;
conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
conf->app_to_use = APPCONF_APP_TO_USE;
@ -683,6 +720,9 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_chuk_conf.multi_esc = APPCONF_CHUK_MULTI_ESC;
conf->app_chuk_conf.tc = APPCONF_CHUK_TC;
conf->app_chuk_conf.tc_max_diff = APPCONF_CHUK_TC_MAX_DIFF;
conf->app_chuk_conf.use_smart_rev = APPCONF_CHUK_USE_SMART_REV;
conf->app_chuk_conf.smart_rev_max_duty = APPCONF_CHUK_SMART_REV_MAX_DUTY;
conf->app_chuk_conf.smart_rev_ramp_time = APPCONF_CHUK_SMART_REV_RAMP_TIME;
conf->app_nrf_conf.speed = APPCONF_NRF_SPEED;
conf->app_nrf_conf.power = APPCONF_NRF_POWER;
conf->app_nrf_conf.crc_type = APPCONF_NRF_CRC;
@ -696,24 +736,38 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.kp = APPCONF_BALANCE_KP;
conf->app_balance_conf.ki = APPCONF_BALANCE_KI;
conf->app_balance_conf.kd = APPCONF_BALANCE_KD;
conf->app_balance_conf.loop_delay = APPCONF_BALANCE_LOOP_DELAY;
conf->app_balance_conf.m_acd = APPCONF_BALANCE_M_ACD;
conf->app_balance_conf.m_b = APPCONF_BALANCE_M_B;
conf->app_balance_conf.cal_delay = APPCONF_BALANCE_CAL_DELAY;
conf->app_balance_conf.cal_m_acd = APPCONF_BALANCE_CAL_M_ACD;
conf->app_balance_conf.cal_m_b = APPCONF_BALANCE_CAL_M_B;
conf->app_balance_conf.pitch_offset = APPCONF_BALANCE_PITCH_OFFSET;
conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET;
conf->app_balance_conf.use_peripheral = APPCONF_BALANCE_USE_PERIPHERAL;
conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT;
conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT;
conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ;
conf->app_balance_conf.m_axis = APPCONF_BALANCE_M_AXIS;
conf->app_balance_conf.c_axis = APPCONF_BALANCE_C_AXIS;
conf->app_balance_conf.m_fault = APPCONF_BALANCE_M_FAULT;
conf->app_balance_conf.c_fault = APPCONF_BALANCE_C_FAULT;
conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY;
conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
conf->app_balance_conf.tiltback_angle = APPCONF_BALANCE_TILTBACK_ANGLE;
conf->app_balance_conf.tiltback_speed = APPCONF_BALANCE_TILTBACK_SPEED;
conf->app_balance_conf.startup_pitch = APPCONF_BALANCE_STARTUP_PITCH;
conf->app_balance_conf.startup_roll = APPCONF_BALANCE_STARTUP_ROLL;
conf->app_balance_conf.startup_m_tolerance = APPCONF_BALANCE_STARTUP_M_TOLERANCE;
conf->app_balance_conf.startup_c_tolerance = APPCONF_BALANCE_STARTUP_C_TOLERANCE;
conf->app_balance_conf.startup_speed = APPCONF_BALANCE_STARTUP_SPEED;
conf->app_balance_conf.deadzone = APPCONF_BALANCE_DEADZONE;
conf->app_balance_conf.current_boost = APPCONF_BALANCE_CURRENT_BOOST;
conf->imu_conf.type = APPCONF_IMU_TYPE;
conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE;
conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ;
conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY;
conf->imu_conf.mahony_kp = APPCONF_IMU_MAHONY_KP;
conf->imu_conf.mahony_ki = APPCONF_IMU_MAHONY_KI;
conf->imu_conf.madgwick_beta = APPCONF_IMU_MADGWICK_BETA;
conf->imu_conf.rot_roll = APPCONF_IMU_ROT_ROLL;
conf->imu_conf.rot_pitch = APPCONF_IMU_ROT_PITCH;
conf->imu_conf.rot_yaw = APPCONF_IMU_ROT_YAW;
conf->imu_conf.accel_offsets[0] = APPCONF_IMU_A_OFFSET_0;
conf->imu_conf.accel_offsets[1] = APPCONF_IMU_A_OFFSET_1;
conf->imu_conf.accel_offsets[2] = APPCONF_IMU_A_OFFSET_2;
conf->imu_conf.gyro_offsets[0] = APPCONF_IMU_G_OFFSET_0;
conf->imu_conf.gyro_offsets[1] = APPCONF_IMU_G_OFFSET_1;
conf->imu_conf.gyro_offsets[2] = APPCONF_IMU_G_OFFSET_2;
conf->imu_conf.gyro_offset_comp_fact[0] = APPCONF_IMU_G_OFFSET_COMP_FACT_0;
conf->imu_conf.gyro_offset_comp_fact[1] = APPCONF_IMU_G_OFFSET_COMP_FACT_1;
conf->imu_conf.gyro_offset_comp_fact[2] = APPCONF_IMU_G_OFFSET_COMP_FACT_2;
conf->imu_conf.gyro_offset_comp_clamp = APPCONF_IMU_G_OFFSET_COMP_CLAMP;
}

2
confgenerator.h
View File

@ -9,7 +9,7 @@
// Constants
#define MCCONF_SIGNATURE 503309878
#define APPCONF_SIGNATURE 3625267250
#define APPCONF_SIGNATURE 3772026662
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

102
datatypes.h
View File

@ -424,6 +424,9 @@ typedef struct {
bool multi_esc;
bool tc;
float tc_max_diff;
bool use_smart_rev;
float smart_rev_max_duty;
float smart_rev_ramp_time;
} chuk_config;
// NRF Datatypes
@ -483,28 +486,28 @@ typedef struct {
bool send_crc_ack;
} nrf_config;
// Balance Datatypes
// External LED state
typedef enum {
PITCH = 0,
ROLL,
YAW
} AXIS;
typedef struct {
float kp;
float ki;
float kd;
uint8_t loop_delay;
float m_acd;
float m_b;
uint32_t cal_delay;
float cal_m_acd;
float cal_m_b;
float pitch_offset;
float roll_offset;
bool use_peripheral;
float pitch_fault;
float roll_fault;
uint16_t hertz;
AXIS m_axis;
AXIS c_axis;
float m_fault;
float c_fault;
float overspeed_duty;
float tiltback_duty;
float tiltback_angle;
float tiltback_speed;
float startup_pitch;
float startup_roll;
float startup_m_tolerance;
float startup_c_tolerance;
float startup_speed;
float deadzone;
float current_boost;
@ -520,6 +523,49 @@ typedef enum {
CAN_STATUS_1_2_3_4_5
} CAN_STATUS_MODE;
typedef enum {
SHUTDOWN_MODE_ALWAYS_OFF = 0,
SHUTDOWN_MODE_ALWAYS_ON,
SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY,
SHUTDOWN_MODE_OFF_AFTER_10S,
SHUTDOWN_MODE_OFF_AFTER_1M,
SHUTDOWN_MODE_OFF_AFTER_5M,
SHUTDOWN_MODE_OFF_AFTER_10M,
SHUTDOWN_MODE_OFF_AFTER_30M,
SHUTDOWN_MODE_OFF_AFTER_1H,
SHUTDOWN_MODE_OFF_AFTER_5H,
} SHUTDOWN_MODE;
typedef enum {
IMU_TYPE_OFF = 0,
IMU_TYPE_INTERNAL,
IMU_TYPE_EXTERNAL_MPU9X50,
IMU_TYPE_EXTERNAL_ICM20948,
IMU_TYPE_EXTERNAL_BMI160
} IMU_TYPE;
typedef enum {
AHRS_MODE_MADGWICK = 0,
AHRS_MODE_MAHONY
} AHRS_MODE;
typedef struct {
IMU_TYPE type;
AHRS_MODE mode;
int sample_rate_hz;
float accel_confidence_decay;
float mahony_kp;
float mahony_ki;
float madgwick_beta;
float rot_roll;
float rot_pitch;
float rot_yaw;
float accel_offsets[3];
float gyro_offsets[3];
float gyro_offset_comp_fact[3];
float gyro_offset_comp_clamp;
} imu_config;
typedef struct {
// Settings
uint8_t controller_id;
@ -530,6 +576,7 @@ typedef struct {
CAN_BAUD can_baud_rate;
bool pairing_done;
bool permanent_uart_enabled;
SHUTDOWN_MODE shutdown_mode;
// UAVCAN
bool uavcan_enable;
@ -553,8 +600,11 @@ typedef struct {
// NRF application settings
nrf_config app_nrf_conf;
// Balance application settings
balance_config app_balance_conf;
// Balance application settings
balance_config app_balance_conf;
// IMU Settings
imu_config imu_conf;
} app_configuration;
// Communication commands
@ -630,7 +680,15 @@ typedef enum {
COMM_BM_ERASE_FLASH_ALL,
COMM_BM_WRITE_FLASH,
COMM_BM_REBOOT,
COMM_BM_DISCONNECT
COMM_BM_DISCONNECT,
COMM_BM_MAP_PINS_DEFAULT,
COMM_BM_MAP_PINS_NRF5X,
COMM_ERASE_BOOTLOADER,
COMM_ERASE_BOOTLOADER_ALL_CAN,
COMM_PLOT_INIT,
COMM_PLOT_DATA,
COMM_PLOT_ADD_GRAPH,
COMM_PLOT_SET_GRAPH,
} COMM_PACKET_ID;
// CAN commands
@ -819,4 +877,14 @@ typedef union {
#define EEPROM_VARS_HW 64
#define EEPROM_VARS_CUSTOM 64
typedef struct {
float ah_tot;
float ah_charge_tot;
float wh_tot;
float wh_charge_tot;
float current_tot;
float current_in_tot;
uint8_t num_vescs;
} setup_values;
#endif /* DATATYPES_H_ */

15
encoder.c
View File

@ -490,19 +490,22 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
//palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
send <<= 1;
spi_delay();
palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
spi_delay();
int r1, r2, r3;
r1 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
int samples = 0;
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
__NOP();
r2 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
__NOP();
r3 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
__NOP();
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
__NOP();
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
recieve <<= 1;
if (utils_middle_of_3_int(r1, r2, r3)) {
if (samples > 2) {
recieve |= 1;
}

29
flash_helper.c
View File

@ -1,5 +1,5 @@
/*
Copyright 2016 Benjamin Vedder benjamin@vedder.se
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
@ -118,14 +118,39 @@ uint16_t flash_helper_erase_new_app(uint32_t new_app_size) {
uint16_t res = FLASH_EraseSector(flash_sector[NEW_APP_BASE + i], VoltageRange_3);
if (res != FLASH_COMPLETE) {
FLASH_Lock();
timeout_configure_IWDT();
utils_sys_unlock_cnt();
return res;
}
} else {
break;
}
}
FLASH_Lock();
FLASH_Lock();
timeout_configure_IWDT();
utils_sys_unlock_cnt();
return FLASH_COMPLETE;
}
uint16_t flash_helper_erase_bootloader(void) {
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
mc_interface_unlock();
mc_interface_release_motor();
utils_sys_lock_cnt();
timeout_configure_IWDT_slowest();
uint16_t res = FLASH_EraseSector(flash_sector[BOOTLOADER_BASE], VoltageRange_3);
if (res != FLASH_COMPLETE) {
FLASH_Lock();
return res;
}
FLASH_Lock();
timeout_configure_IWDT();
utils_sys_unlock_cnt();

3
flash_helper.h
View File

@ -1,5 +1,5 @@
/*
Copyright 2016 Benjamin Vedder benjamin@vedder.se
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
@ -24,6 +24,7 @@
// Functions
uint16_t flash_helper_erase_new_app(uint32_t new_app_size);
uint16_t flash_helper_erase_bootloader(void);
uint16_t flash_helper_write_new_app_data(uint32_t offset, uint8_t *data, uint32_t len);
void flash_helper_jump_to_bootloader(void);
uint8_t* flash_helper_get_sector_address(uint32_t fsector);

4
hwconf/drv8323s.c
View File

@ -55,8 +55,8 @@ void drv8323s_init(void) {
chThdSleepMilliseconds(100);
// Disable OC
drv8323s_write_reg(5, 0x04C0);
// Disable OC 0000TTDDMMOOVVVV
drv8323s_write_reg(5, 0b0000000111010000);
// Set shunt amp gain
drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN);

4
hwconf/hw.h
View File

@ -329,6 +329,10 @@
#define HW_PERMANENT_NRF_FAILED_HOOK()
#endif
#ifndef HW_EARLY_INIT
#define HW_EARLY_INIT()
#endif
// Default ID
#ifndef HW_DEFAULT_ID
#define HW_DEFAULT_ID (APPCONF_CONTROLLER_ID >= 0 ? APPCONF_CONTROLLER_ID : hw_id_from_uuid())

4
hwconf/hw_40.h
View File

@ -51,7 +51,7 @@
* 6: VREFINT
* 7: IN11 NC
* 8: IN12 AN_IN
* 9: IN13 NC
* 9: IN13 NC (TEMP_MOSFET to make it compile)
* 10: IN15 ADC_EXT
* 11: IN10 TEMP_MOTOR
*/
@ -68,7 +68,9 @@
#define ADC_IND_CURR2 3
#define ADC_IND_VIN_SENS 8
#define ADC_IND_EXT 10
#define ADC_IND_TEMP_MOS 9
#define ADC_IND_VREFINT 6
#define ADC_IND_TEMP_MOTOR 11
// ADC macros and settings

3
hwconf/hw_45.h
View File

@ -51,7 +51,7 @@
* 6: VREFINT
* 7: IN11 NC
* 8: IN12 AN_IN
* 9: IN13 NC
* 9: IN13 NC (TEMP_MOSFET to make it compile)
* 10: IN15 ADC_EXT
* 11: IN10 TEMP_MOTOR
*/
@ -68,6 +68,7 @@
#define ADC_IND_CURR2 3
#define ADC_IND_VIN_SENS 8
#define ADC_IND_EXT 10
#define ADC_IND_TEMP_MOS 9
#define ADC_IND_VREFINT 6
#define ADC_IND_TEMP_MOTOR 11

300
hwconf/hw_hd.c Normal file
View File

@ -0,0 +1,300 @@
/*
Copyright 2019 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/>.
*/
#include "hw.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "utils.h"
#include "drv8323s.h"
#include "terminal.h"
#include "commands.h"
#include "mc_interface.h"
// Variables
static volatile bool i2c_running = false;
static mutex_t shutdown_mutex;
static float bt_diff = 0.0;
// I2C configuration
static const I2CConfig i2cfg = {
OPMODE_I2C,
100000,
STD_DUTY_CYCLE
};
// Private functions
static void terminal_shutdown_now(int argc, const char **argv);
static void terminal_button_test(int argc, const char **argv);
void hw_init_gpio(void) {
chMtxObjectInit(&shutdown_mutex);
// 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, 0,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOB, 1,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// ENABLE_GATE
palSetPadMode(GPIOB, 5,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// Disable DCCAL
palSetPadMode(GPIOD, 2,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palClearPad(GPIOD, 2);
ENABLE_GATE();
// 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);
// Fault pin
palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
// 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(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);
drv8323s_init();
terminal_register_command_callback(
"shutdown",
"Shutdown VESC now.",
0,
terminal_shutdown_now);
terminal_register_command_callback(
"test_button",
"Try sampling the shutdown button",
0,
terminal_button_test);
}
void hw_setup_adc_channels(void) {
// ADC1 regular channels
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
// ADC2 regular channels
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_0, 5, ADC_SampleTime_15Cycles);
// ADC3 regular channels
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_1, 5, ADC_SampleTime_15Cycles);
// Injected channels
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
}
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);
}
}
bool hw_sample_shutdown_button(void) {
chMtxLock(&shutdown_mutex);
bt_diff = 0.0;
for (int i = 0;i < 3;i++) {
palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
chThdSleep(5);
float val1 = ADC_VOLTS(ADC_IND_SHUTDOWN);
chThdSleepMilliseconds(1);
float val2 = ADC_VOLTS(ADC_IND_SHUTDOWN);
palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL);
chThdSleepMilliseconds(1);
bt_diff += (val1 - val2);
}
chMtxUnlock(&shutdown_mutex);
return (bt_diff > 0.12);
}
static void terminal_shutdown_now(int argc, const char **argv) {
(void)argc;
(void)argv;
DISABLE_GATE();
HW_SHUTDOWN_HOLD_OFF();
}
static void terminal_button_test(int argc, const char **argv) {
(void)argc;
(void)argv;
for (int i = 0;i < 40;i++) {
commands_printf("BT: %d %.2f", HW_SAMPLE_SHUTDOWN(), (double)bt_diff);
chThdSleepMilliseconds(100);
}
}

275
hwconf/hw_hd.h Normal file
View File

@ -0,0 +1,275 @@
/*
Copyright 2019 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_HD_H_
#define HW_HD_H_
#include "drv8323s.h"
#define HW_NAME "HD"
// HW properties
#define HW_HAS_DRV8323S
#define HW_HAS_3_SHUNTS
// Macros
#define ENABLE_GATE() palSetPad(GPIOB, 5)
#define DISABLE_GATE() palClearPad(GPIOB, 5)
#define IS_DRV_FAULT() (!palReadPad(GPIOB, 7))
#define LED_GREEN_ON() palSetPad(GPIOB, 0)
#define LED_GREEN_OFF() palClearPad(GPIOB, 0)
#define LED_RED_ON() palSetPad(GPIOB, 1)
#define LED_RED_OFF() palClearPad(GPIOB, 1)
// Shutdown pin
#define HW_SHUTDOWN_GPIO GPIOC
#define HW_SHUTDOWN_PIN 5
#define HW_SHUTDOWN_HOLD_ON() palSetPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SHUTDOWN_HOLD_OFF() palClearPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SAMPLE_SHUTDOWN() hw_sample_shutdown_button()
// Hold shutdown pin early to wake up on short pulses
#define HW_EARLY_INIT() palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL); \
HW_SHUTDOWN_HOLD_ON();
/*
* ADC Vector
*
* 0: IN0 SENS1
* 1: IN1 SENS2
* 2: IN2 SENS3
* 3: IN10 CURR1
* 4: IN11 CURR2
* 5: IN12 CURR3
* 6: IN5 ADC_EXT1
* 7: IN6 ADC_EXT2
* 8: IN3 TEMP_PCB
* 9: IN14 TEMP_MOTOR
* 10: IN15 Shutdown
* 11: IN13 AN_IN
* 12: Vrefint
* 13: IN0 SENS1
* 14: IN1 SENS2
*/
#define HW_ADC_CHANNELS 15
#define HW_ADC_INJ_CHANNELS 3
#define HW_ADC_NBR_CONV 5
// ADC Indexes
#define ADC_IND_SENS1 0
#define ADC_IND_SENS2 1
#define ADC_IND_SENS3 2
#define ADC_IND_CURR1 3
#define ADC_IND_CURR2 4
#define ADC_IND_CURR3 5
#define ADC_IND_VIN_SENS 11
#define ADC_IND_EXT 6
#define ADC_IND_EXT2 7
#define ADC_IND_TEMP_MOS 8
#define ADC_IND_TEMP_MOTOR 9
#define ADC_IND_VREFINT 12
#define ADC_IND_SHUTDOWN 10
// ADC macros and settings
// Component parameters (can be overridden)
#ifndef V_REG
#define V_REG 3.3
#endif
#ifndef VIN_R1
#define VIN_R1 39000.0
#endif
#ifndef VIN_R2
#define VIN_R2 2200.0
#endif
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 20.0
#endif
#ifndef CURRENT_SHUNT_RES
#define CURRENT_SHUNT_RES 0.0005
#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) ((4095.0 * 10000.0) / adc_val - 10000.0)
#define NTC_TEMP(adc_ind) (1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
#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)
// Voltage on ADC channel
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
// Double samples in beginning and end for positive current measurement.
// Useful when the shunt sense traces have noise that causes offset.
#ifndef CURR1_DOUBLE_SAMPLE
#define CURR1_DOUBLE_SAMPLE 0
#endif
#ifndef CURR2_DOUBLE_SAMPLE
#define CURR2_DOUBLE_SAMPLE 0
#endif
#ifndef CURR3_DOUBLE_SAMPLE
#define CURR3_DOUBLE_SAMPLE 0
#endif
// UART Peripheral
#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
// Permanent UART Peripheral (for NRF51)
#define HW_UART_P_BAUD 115200
#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 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
// 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
// SPI for DRV8323S
#define DRV8323S_MOSI_GPIO GPIOC
#define DRV8323S_MOSI_PIN 12
#define DRV8323S_MISO_GPIO GPIOB
#define DRV8323S_MISO_PIN 3
#define DRV8323S_SCK_GPIO GPIOB
#define DRV8323S_SCK_PIN 4
#define DRV8323S_CS_GPIO GPIOC
#define DRV8323S_CS_PIN 9
// BMI160
#define BMI160_SDA_GPIO GPIOB
#define BMI160_SDA_PIN 2
#define BMI160_SCL_GPIO GPIOA
#define BMI160_SCL_PIN 15
#define IMU_FLIP
// NRF SWD
#define NRF5x_SWDIO_GPIO GPIOB
#define NRF5x_SWDIO_PIN 12
#define NRF5x_SWCLK_GPIO GPIOA
#define NRF5x_SWCLK_PIN 4
// 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
#ifndef MCCONF_L_CURRENT_MAX
#define MCCONF_L_CURRENT_MAX 60.0 // Current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_CURRENT_MIN
#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_IN_CURRENT_MAX
#define MCCONF_L_IN_CURRENT_MAX 60.0 // Input current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_IN_CURRENT_MIN
#define MCCONF_L_IN_CURRENT_MIN -60.0 // Input current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_MAX_ABS_CURRENT
#define MCCONF_L_MAX_ABS_CURRENT 120.0 // The maximum absolute current above which a fault is generated
#endif
#ifndef MCCONF_M_DRV8301_OC_ADJ
#define MCCONF_M_DRV8301_OC_ADJ 10 // DRV8301 over current protection threshold
#endif
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
#endif
#ifndef MCCONF_FOC_F_SW
#define MCCONF_FOC_F_SW 30000.0
#endif
// Setting limits
#define HW_LIM_CURRENT -100.0, 100.0
#define HW_LIM_CURRENT_IN -100.0, 100.0
#define HW_LIM_CURRENT_ABS 0.0, 150.0
#define HW_LIM_VIN 6.0, 57.0
#define HW_LIM_ERPM -200e3, 200e3
#define HW_LIM_DUTY_MIN 0.0, 0.1
#define HW_LIM_DUTY_MAX 0.0, 0.99
#define HW_LIM_TEMP_FET -40.0, 110.0
// Functions
bool hw_sample_shutdown_button(void);
#endif /* HW_UAVC_QCUBE_H_ */

0
hwconf/hw_uavc_basic.c → hwconf/hw_uavc_omega.c
View File

8
hwconf/hw_uavc_basic.h → hwconf/hw_uavc_omega.h
View File

@ -17,12 +17,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef HW_UAVC_BASIC_H_
#define HW_UAVC_BASIC_H_
#ifndef HW_UAVC_OMEGA_H_
#define HW_UAVC_OMEGA_H_
#include "drv8323s.h"
#define HW_NAME "UAVC_BASIC"
#define HW_NAME "UAVC_OMEGA"
// HW properties
#define HW_HAS_DRV8323S
@ -244,4 +244,4 @@
#define HW_LIM_DUTY_MAX 0.0, 0.99
#define HW_LIM_TEMP_FET -40.0, 110.0
#endif /* HW_UAVC_QCUBE_H_ */
#endif /* HW_UAVC_OMEGA_H_ */

39
imu/BMI160_driver/LICENSE Normal file
View File

@ -0,0 +1,39 @@
Copyright (C) 2015 - 2016 Bosch Sensortec GmbH
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of the
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
The information provided is believed to be accurate and reliable.
The copyright holder assumes no responsibility
for the consequences of use
of such information nor for any infringement of patents or
other rights of third parties which may result from its use.
No license is granted by implication or otherwise under any patent or
patent rights of the copyright holder.

793
imu/BMI160_driver/README.md Normal file
View File

@ -0,0 +1,793 @@
# BMI160 sensor API
## Introduction
This package contains the Bosch Sensortec's BMI160 sensor driver (sensor API)
The sensor driver package includes bmi160.h, bmi160.c and bmi160_defs.h files
## Version
File | Version | Date
--------------|---------|---------------
bmi160.c | 3.7.7 | 13 Mar 2019
bmi160.h | 3.7.7 | 13 Mar 2019
bmi160_defs.h | 3.7.7 | 13 Mar 2019
## Integration details
* Integrate bmi160.h, bmi160_defs.h and bmi160.c file in to your project.
* Include the bmi160.h file in your code like below.
``` c
#include "bmi160.h"
```
## File information
* bmi160_defs.h : This header file has the constants, macros and datatype declarations.
* bmi160.h : This header file contains the declarations of the sensor driver APIs.
* bmi160.c : This source file contains the definitions of the sensor driver APIs.
## Supported sensor interface
* SPI 4-wire
* I2C
## Usage guide
### Initializing the sensor
To initialize the sensor, you will first need to create a device structure. You
can do this by creating an instance of the structure bmi160_dev. Then go on to
fill in the various parameters as shown below.
#### Example for SPI 4-Wire
``` c
struct bmi160_dev sensor;
/* You may assign a chip select identifier to be handled later */
sensor.id = 0;
sensor.interface = BMI160_SPI_INTF;
sensor.read = user_spi_read;
sensor.write = user_spi_write;
sensor.delay_ms = user_delay_ms;
int8_t rslt = BMI160_OK;
rslt = bmi160_init(&sensor);
/* After the above function call, accel_cfg and gyro_cfg parameters in the device
structure are set with default values, found in the datasheet of the sensor */
```
#### Example for I2C
``` c
struct bmi160_dev sensor;
sensor.id = BMI160_I2C_ADDR;
sensor.interface = BMI160_I2C_INTF;
sensor.read = user_i2c_read;
sensor.write = user_i2c_write;
sensor.delay_ms = user_delay_ms;
int8_t rslt = BMI160_OK;
rslt = bmi160_init(&sensor);
/* After the above function call, accel and gyro parameters in the device structure
are set with default values, found in the datasheet of the sensor */
```
### Configuring accel and gyro sensor
#### Example for configuring accel and gyro sensors in normal mode
``` c
int8_t rslt = BMI160_OK;
/* Select the Output data rate, range of accelerometer sensor */
sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ;
sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G;
sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
/* Select the power mode of accelerometer sensor */
sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
/* Select the Output data rate, range of Gyroscope sensor */
sensor.gyro_cfg.odr = BMI160_GYRO_ODR_3200HZ;
sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
/* Select the power mode of Gyroscope sensor */
sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
/* Set the sensor configuration */
rslt = bmi160_set_sens_conf(&sensor);
```
### Reading sensor data
#### Example for reading sensor data
``` c
int8_t rslt = BMI160_OK;
struct bmi160_sensor_data accel;
struct bmi160_sensor_data gyro;
/* To read only Accel data */
rslt = bmi160_get_sensor_data(BMI160_ACCEL_SEL, &accel, NULL, &sensor);
/* To read only Gyro data */
rslt = bmi160_get_sensor_data(BMI160_GYRO_SEL, NULL, &gyro, &sensor);
/* To read both Accel and Gyro data */
bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &accel, &gyro, &sensor);
/* To read Accel data along with time */
rslt = bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_TIME_SEL) , &accel, NULL, &sensor);
/* To read Gyro data along with time */
rslt = bmi160_get_sensor_data((BMI160_GYRO_SEL | BMI160_TIME_SEL), NULL, &gyro, &sensor);
/* To read both Accel and Gyro data along with time*/
bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL | BMI160_TIME_SEL), &accel, &gyro, &sensor);
```
### Setting the power mode of sensors
#### Example for setting power mode of accel and gyro
``` c
int8_t rslt = BMI160_OK;
/* Select the power mode */
sensor.accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE;
sensor.gyro_cfg.power = BMI160_GYRO_FASTSTARTUP_MODE;
/* Set the Power mode */
rslt = bmi160_set_power_mode(&sensor);
/* Select the power mode */
sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
/* Set the Power mode */
rslt = bmi160_set_power_mode(&sensor);
```
### Reading sensor data register
#### Example for reading Chip Address
``` c
int8_t rslt = BMI160_OK;
uint8_t reg_addr = BMI160_CHIP_ID_ADDR;
uint8_t data;
uint16_t len = 1;
rslt = bmi160_get_regs(reg_addr, &data, len, &sensor);
```
### Writing to sensor data register
#### Example for writing data to any motion threshold register
``` c
int8_t rslt = BMI160_OK;
uint8_t reg_addr = BMI160_INT_MOTION_1_ADDR;
uint8_t data = 20;
uint16_t len = 1;
rslt = bmi160_set_regs(reg_addr, &data, len, &sensor);
```
### Resetting the device using soft-reset
#### Example for writing soft-reset command to command register
``` c
int8_t rslt = BMI160_OK;
rslt = bmi160_soft_reset(&sensor);
```
### Configuring interrupts for sensors
To configure the sensor interrupts, you will first need to create an interrupt
structure. You can do this by creating an instance of the structure bmi160_int_settg.
Then go on to fill in the various parameters as shown below
### Configuring Any-motion Interrupt
#### Example for configuring Any-motion Interrupt
Note:- User can check the currently active interrupt(any-motion or sig-motion) by checking the **any_sig_sel** of bmi160_dev structure.
``` c
struct bmi160_int_settg int_config;
/* Select the Interrupt channel/pin */
int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1
/* Select the Interrupt type */
int_config.int_type = BMI160_ACC_ANY_MOTION_INT;// Choosing Any motion interrupt
/* Select the interrupt channel/pin settings */
int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin
int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin
int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output
int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output
int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input
int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output
/* Select the Any-motion interrupt parameters */
int_config.int_type_cfg.acc_any_motion_int.anymotion_en = BMI160_ENABLE;// 1- Enable the any-motion, 0- disable any-motion
int_config.int_type_cfg.acc_any_motion_int.anymotion_x = BMI160_ENABLE;// Enabling x-axis for any motion interrupt
int_config.int_type_cfg.acc_any_motion_int.anymotion_y = BMI160_ENABLE;// Enabling y-axis for any motion interrupt
int_config.int_type_cfg.acc_any_motion_int.anymotion_z = BMI160_ENABLE;// Enabling z-axis for any motion interrupt
int_config.int_type_cfg.acc_any_motion_int.anymotion_dur = 0;// any-motion duration
int_config.int_type_cfg.acc_any_motion_int.anymotion_thr = 20;// (2-g range) -> (slope_thr) * 3.91 mg, (4-g range) -> (slope_thr) * 7.81 mg, (8-g range) ->(slope_thr) * 15.63 mg, (16-g range) -> (slope_thr) * 31.25 mg
/* Set the Any-motion interrupt */
bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */
```
### Configuring Flat Interrupt
#### Example for configuring Flat Interrupt
``` c
struct bmi160_int_settg int_config;
/* Select the Interrupt channel/pin */
int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1
/* Select the Interrupt type */
int_config.int_type = BMI160_ACC_FLAT_INT;// Choosing flat interrupt
/* Select the interrupt channel/pin settings */
int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin
int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin
int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output
int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output
int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input
int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output
/* Select the Flat interrupt parameters */
int_config.int_type_cfg.acc_flat_int.flat_en = BMI160_ENABLE;// 1-enable, 0-disable the flat interrupt
int_config.int_type_cfg.acc_flat_int.flat_theta = 8;// threshold for detection of flat position in range from 0° to 44.8°.
int_config.int_type_cfg.acc_flat_int.flat_hy = 1;// Flat hysteresis
int_config.int_type_cfg.acc_flat_int.flat_hold_time = 1;// Flat hold time (0 -> 0 ms, 1 -> 640 ms, 2 -> 1280 ms, 3 -> 2560 ms)
/* Set the Flat interrupt */
bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */
```
### Configuring Step Detector Interrupt
#### Example for configuring Step Detector Interrupt
``` c
struct bmi160_int_settg int_config;
/* Select the Interrupt channel/pin */
int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1
/* Select the Interrupt type */
int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt
/* Select the interrupt channel/pin settings */
int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin
int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin
int_config.int_pin_settg.output_type = BMI160_ENABLE;// Choosing active High output
int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output
int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input
int_config.int_pin_settg.latch_dur =BMI160_LATCH_DUR_NONE;// non-latched output
/* Select the Step Detector interrupt parameters, Kindly use the recommended settings for step detector */
int_config.int_type_cfg.acc_step_detect_int.step_detector_mode = BMI160_STEP_DETECT_NORMAL;
int_config.int_type_cfg.acc_step_detect_int.step_detector_en = BMI160_ENABLE;// 1-enable, 0-disable the step detector
/* Set the Step Detector interrupt */
bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */
```
### Configuring Step counter
To configure the step counter, user need to configure the step detector interrupt as described in above section.
After configuring step detector, see the below code snippet for user space & ISR
### User space
``` c
int8_t rslt = BMI160_OK;
uint8_t step_enable = 1;//enable the step counter
rslt = bmi160_set_step_counter(step_enable, &sensor);
```
### ISR
``` c
int8_t rslt = BMI160_OK;
uint16_t step_count = 0;//stores the step counter value
rslt = bmi160_read_step_counter(&step_count, &sensor);
```
### Unmapping Interrupt
#### Example for unmapping Step Detector Interrupt
``` c
struct bmi160_int_settg int_config;
/* Deselect the Interrupt channel/pin */
int_config.int_channel = BMI160_INT_CHANNEL_NONE;
/* Select the Interrupt type */
int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt
/* Set the Step Detector interrupt */
bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */
```
### Reading interrupt status
#### Example for reading interrupt status for step detector
``` c
union bmi160_int_status interrupt;
enum bmi160_int_status_sel int_status_sel;
/* Interrupt status selection to read all interrupts */
int_status_sel = BMI160_INT_STATUS_ALL;
rslt = bmi160_get_int_status(int_status_sel, &interrupt, &sensor);
if (interrupt.bit.step)
printf("Step detector interrupt occured\n");
```
### Configuring the auxiliary sensor BMM150
It is assumed that secondary interface of bmi160 has external pull-up resistor in order to access the auxiliary sensor bmm150.
### Accessing auxiliary BMM150 with BMM150 APIs via BMI160 secondary interface.
## Integration details
* Integrate the souce codes of BMM150 and BMI160 in project.
* Include the bmi160.h and bmm150.h file in your code like below.
* It is mandatory to initialize the bmi160 device structure for primary interface and auxiliary sensor settings.
* Create two wrapper functions , user_aux_read and user_aux_write in order to match the signature as mentioned below.
* Invoke the "bmi160_aux_init" API to initialise the secondary interface in BMI160.
* Invoke the "bmm150_init" API to initialise the BMM150 sensor.
* Now we can use the BMM150 sensor APIs to access the BMM150 via BMI160.
``` c
/* main.c file */
#include "bmi160.h"
#include "bmm150.h"
```
### Initialization of auxiliary sensor BMM150
```
/* main.c file */
struct bmm150_dev bmm150;
/* function declaration */
int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len);
int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len);
/* Configure device structure for auxiliary sensor parameter */
sensor.aux_cfg.aux_sensor_enable = 1; // auxiliary sensor enable
sensor.aux_cfg.aux_i2c_addr = BMI160_AUX_BMM150_I2C_ADDR; // auxiliary sensor address
sensor.aux_cfg.manual_enable = 1; // setup mode enable
sensor.aux_cfg.aux_rd_burst_len = 2;// burst read of 2 byte
/* Configure the BMM150 device structure by
mapping user_aux_read and user_aux_write */
bmm150.read = user_aux_read;
bmm150.write = user_aux_write;
bmm150.id = BMM150_DEFAULT_I2C_ADDRESS;
/* Ensure that sensor.aux_cfg.aux_i2c_addr = bmm150.id
for proper sensor operation */
bmm150.delay_ms = delay_ms;
bmm150.interface = BMM150_I2C_INTF;
/* Initialize the auxiliary sensor interface */
rslt = bmi160_aux_init(&sensor);
/* Auxiliary sensor is enabled and can be accessed from this point */
/* Configure the desired settings in auxiliary BMM150 sensor
* using the bmm150 APIs */
/* Initialising the bmm150 sensor */
rslt = bmm150_init(&bmm150);
/* Set the power mode and preset mode to enable Mag data sampling */
bmm150.settings.pwr_mode = BMM150_NORMAL_MODE;
rslt = bmm150_set_op_mode(&bmm150);
bmm150.settings.preset_mode= BMM150_PRESETMODE_LOWPOWER;
rslt = bmm150_set_presetmode(&bmm150);
```
### Wrapper functions
```
/*wrapper function to match the signature of bmm150.read */
int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len)
{
int8_t rslt;
/* Discarding the parameter id as it is redundant*/
rslt = bmi160_aux_read(reg_addr, aux_data, len, &bmi160);
return rslt;
}
/*wrapper function to match the signature of bmm150.write */
int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len)
{
int8_t rslt;
/* Discarding the parameter id as it is redundant */
rslt = bmi160_aux_write(reg_addr, aux_data, len, &bmi160);
return rslt;
}
```
### Initialization of auxiliary BMM150 in auto mode
Any sensor whose data bytes are less than or equal to 8 bytes can be synchronized with the BMI160
and read out of Accelerometer + Gyroscope + Auxiliary sensor data of that instance is possible
which helps in creating less latency fusion data
```
/* Initialize the Auxiliary BMM150 following the above code
* until setting the power mode (Set the power mode as forced mode)
* and preset mode */
/* In BMM150 Mag data starts from register address 0x42 */
uint8_t aux_addr = 0x42;
/* Buffer to store the Mag data from 0x42 to 0x48 */
uint8_t mag_data[8] = {0};
uint8_t index;
/* Configure the Auxiliary sensor either in auto/manual modes and set the
polling frequency for the Auxiliary interface */
sensor.aux_cfg.aux_odr = 8; /* Represents polling rate in 100 Hz*/
rslt = bmi160_config_aux_mode(&sensor)
/* Set the auxiliary sensor to auto mode */
rslt = bmi160_set_aux_auto_mode(&aux_addr, &sensor);
/* Reading data from BMI160 data registers */
rslt = bmi160_read_aux_data_auto_mode(mag_data, &sensor);
printf("\n RAW DATA ");
for(index = 0 ; index < 8 ; index++)
{
printf("\n MAG DATA[%d] : %d ", index, mag_data[index]);
}
/* Compensating the raw mag data available from the BMM150 API */
rslt = bmm150_aux_mag_data(mag_data, &bmm150);
printf("\n COMPENSATED DATA ");
printf("\n MAG DATA X : %d Y : %d Z : %d", bmm150.data.x, bmm150.data.y, bmm150.data.z);
```
### Auxiliary FIFO data parsing
The Auxiliary sensor data can be stored in FIFO , Here we demonstrate an example for
using the Bosch Magnetometer sensor BMM150 and storing its data in FIFO
```
/* Initialize the Aux BMM150 following the above
* code and by creating the Wrapper functions */
int8_t rslt = 0;
uint8_t aux_instance = 0;
uint16_t fifo_cnt = 0;
uint8_t auto_mode_addr;
uint8_t i;
/* Setup and configure the FIFO buffer */
/* Declare memory to store the raw FIFO buffer information */
uint8_t fifo_buff[1000] = {0};
/* Modify the FIFO buffer instance and link to the device instance */
struct bmi160_fifo_frame fifo_frame;
fifo_frame.data = fifo_buff;
fifo_frame.length = 1000;
dev->fifo = &fifo_frame;
/* Declare instances of the sensor data structure to store the parsed FIFO data */
struct bmi160_aux_data aux_data[112]; //1000 / 9 bytes per frame ~ 111 data frames
rslt = bmi160_init(dev);
printf("\n BMI160 chip ID is : %d ",dev->chip_id);
rslt = bmi160_aux_init(dev);
rslt = bmm150_init(&bmm150);
printf("\n BMM150 CHIP ID : %d",bmm150.chip_id);
bmm150.settings.preset_mode = BMM150_PRESETMODE_LOWPOWER;
rslt = bmm150_set_presetmode(&bmm150);
bmm150.settings.pwr_mode = BMM150_FORCED_MODE;
rslt = bmm150_set_op_mode(&bmm150);
/* Enter the data register of BMM150 to "auto_mode_addr" here it is 0x42 */
auto_mode_addr = 0x42;
printf("\n ENTERING AUX. AUTO MODE ");
dev->aux_cfg.aux_odr = BMI160_AUX_ODR_25HZ;
rslt = bmi160_set_aux_auto_mode(&auto_mode_addr, dev);
/* Disable other FIFO settings */
rslt = bmi160_set_fifo_config(BMI160_FIFO_CONFIG_1_MASK , BMI160_DISABLE, dev);
/* Enable the required FIFO settings */
rslt = bmi160_set_fifo_config(BMI160_FIFO_AUX | BMI160_FIFO_HEADER, BMI160_ENABLE, dev);
/* Delay for the FIFO to get filled */
dev->delay_ms(400);
printf("\n FIFO DATA REQUESTED (in bytes): %d",dev->fifo->length);
rslt = bmi160_get_fifo_data(dev);
printf("\n FIFO DATA AVAILABLE (in bytes): %d",dev->fifo->length);
/* Print the raw FIFO data obtained */
for(fifo_cnt = 0; fifo_cnt < dev->fifo->length ; fifo_cnt++) {
printf("\n FIFO DATA [%d] IS : %x ",fifo_cnt ,dev->fifo->data[fifo_cnt]);
}
printf("\n\n----------------------------------------------------\n");
/* Set the number of required sensor data instances */
aux_instance = 150;
/* Extract the aux data , 1frame = 8 data bytes */
printf("\n AUX DATA REQUESTED TO BE EXTRACTED (in frames): %d",aux_instance);
rslt = bmi160_extract_aux(aux_data, &aux_instance, dev);
printf("\n AUX DATA ACTUALLY EXTRACTED (in frames): %d",aux_instance);
/* Printing the raw aux data */
for (i = 0; i < aux_instance; i++) {
printf("\n Aux data[%d] : %x",i,aux_data[i].data[0]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[1]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[2]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[3]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[4]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[5]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[6]);
printf("\n Aux data[%d] : %x",i,aux_data[i].data[7]);
}
printf("\n\n----------------------------------------------------\n");
/* Compensate the raw mag data using BMM150 API */
for (i = 0; i < aux_instance; i++) {
printf("\n----------------------------------------------------");
printf("\n Aux data[%d] : %x , %x , %x , %x , %x , %x , %x , %x",i
,aux_data[i].data[0],aux_data[i].data[1]
,aux_data[i].data[2],aux_data[i].data[3]
,aux_data[i].data[4],aux_data[i].data[5]
,aux_data[i].data[6],aux_data[i].data[7]);
/* Compensated mag data using BMM150 API */
rslt = bmm150_aux_mag_data(&aux_data[i].data[0], &bmm150);
/* Printing the Compensated mag data */
if (rslt == BMM150_OK) {
printf("\n MAG DATA COMPENSATION USING BMM150 APIs");
printf("\n COMPENSATED DATA ");
printf("\n MAG DATA X : %d Y : %d Z : %d"
, bmm150.data.x, bmm150.data.y, bmm150.data.z);
} else {
printf("\n MAG DATA COMPENSATION IN BMM150 API is FAILED ");
}
printf("\n----------------------------------------------------\n");
}
```
## Self-test
#### Example for performing accel self test
```
/* Call the "bmi160_init" API as a prerequisite before performing self test
* since invoking self-test will reset the sensor */
rslt = bmi160_perform_self_test(BMI160_ACCEL_ONLY, sen);
/* Utilize the enum BMI160_GYRO_ONLY instead of BMI160_ACCEL_ONLY
to perform self test for gyro */
if (rslt == BMI160_OK) {
printf("\n ACCEL SELF TEST RESULT SUCCESS);
} else {
printf("\n ACCEL SELF TEST RESULT FAIL);
}
```
## FIFO
#### Example for reading FIFO and extracting Gyro data in Header mode
```
/* An example to read the Gyro data in header mode along with sensor time (if available)
* Configure the gyro sensor as prerequisite and follow the below example to read and
* obtain the gyro data from FIFO */
int8_t fifo_gyro_header_time_data(struct bmi160_dev *dev)
{
int8_t rslt = 0;
/* Declare memory to store the raw FIFO buffer information */
uint8_t fifo_buff[300];
/* Modify the FIFO buffer instance and link to the device instance */
struct bmi160_fifo_frame fifo_frame;
fifo_frame.data = fifo_buff;
fifo_frame.length = 300;
dev->fifo = &fifo_frame;
uint16_t index = 0;
/* Declare instances of the sensor data structure to store the parsed FIFO data */
struct bmi160_sensor_data gyro_data[42]; // 300 bytes / ~7bytes per frame ~ 42 data frames
uint8_t gyro_frames_req = 42;
uint8_t gyro_index;
/* Configure the sensor's FIFO settings */
rslt = bmi160_set_fifo_config(BMI160_FIFO_GYRO | BMI160_FIFO_HEADER | BMI160_FIFO_TIME,
BMI160_ENABLE, dev);
if (rslt == BMI160_OK) {
/* At ODR of 100 Hz ,1 frame gets updated in 1/100 = 0.01s
i.e. for 42 frames we need 42 * 0.01 = 0.42s = 420ms delay */
dev->delay_ms(420);
/* Read data from the sensor's FIFO and store it the FIFO buffer,"fifo_buff" */
printf("\n USER REQUESTED FIFO LENGTH : %d\n",dev->fifo->length);
rslt = bmi160_get_fifo_data(dev);
if (rslt == BMI160_OK) {
printf("\n AVAILABLE FIFO LENGTH : %d\n",dev->fifo->length);
/* Print the raw FIFO data */
for (index = 0; index < dev->fifo->length; index++) {
printf("\n FIFO DATA INDEX[%d] = %d", index,
dev->fifo->data[index]);
}
/* Parse the FIFO data to extract gyro data from the FIFO buffer */
printf("\n REQUESTED GYRO DATA FRAMES : %d\n ",gyro_frames_req);
rslt = bmi160_extract_gyro(gyro_data, &gyro_frames_req, dev);
if (rslt == BMI160_OK) {
printf("\n AVAILABLE GYRO DATA FRAMES : %d\n ",gyro_frames_req);
/* Print the parsed gyro data from the FIFO buffer */
for (gyro_index = 0; gyro_index < gyro_frames_req; gyro_index++) {
printf("\nFIFO GYRO FRAME[%d]",gyro_index);
printf("\nGYRO X-DATA : %d \t Y-DATA : %d \t Z-DATA : %d"
,gyro_data[gyro_index].x ,gyro_data[gyro_index].y
,gyro_data[gyro_index].z);
}
/* Print the special FIFO frame data like sensortime */
printf("\n SENSOR TIME DATA : %d \n",dev->fifo->sensor_time);
printf("SKIPPED FRAME COUNT : %d",dev->fifo->skipped_frame_count);
} else {
printf("\n Gyro data extraction failed");
}
} else {
printf("\n Reading FIFO data failed");
}
} else {
printf("\n Setting FIFO configuration failed");
}
return rslt;
}
```
## FOC and offset compensation
> FOC shouldnot be used in Low-power mode
#### Example for configuring FOC for accel and gyro
```
/* An example for configuring FOC for accel and gyro data */
int8_t start_foc(struct bmi160_dev *dev)
{
int8_t rslt = 0;
/* FOC configuration structure */
struct bmi160_foc_conf foc_conf;
/* Structure to store the offsets */
struct bmi160_offsets offsets;
/* Enable FOC for accel with target values of z = 1g ; x,y as 0g */
foc_conf.acc_off_en = BMI160_ENABLE;
foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G;
foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G;
foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G;
/* Enable FOC for gyro */
foc_conf.foc_gyr_en = BMI160_ENABLE;
foc_conf.gyro_off_en = BMI160_ENABLE;
rslt = bmi160_start_foc(&foc_conf, &offsets, sen);
if (rslt == BMI160_OK) {
printf("\n FOC DONE SUCCESSFULLY ");
printf("\n OFFSET VALUES AFTER FOC : ");
printf("\n OFFSET VALUES ACCEL X : %d ",offsets.off_acc_x);
printf("\n OFFSET VALUES ACCEL Y : %d ",offsets.off_acc_y);
printf("\n OFFSET VALUES ACCEL Z : %d ",offsets.off_acc_z);
printf("\n OFFSET VALUES GYRO X : %d ",offsets.off_gyro_x);
printf("\n OFFSET VALUES GYRO Y : %d ",offsets.off_gyro_y);
printf("\n OFFSET VALUES GYRO Z : %d ",offsets.off_gyro_z);
}
/* After start of FOC offsets will be updated automatically and
* the data will be very much close to the target values of measurement */
return rslt;
}
```
#### Example for updating the offsets manually
> The offsets set by this method will be reset on soft-reset/POR
```
/* An example for updating manual offsets to sensor */
int8_t write_offsets(struct bmi160_dev *dev)
{
int8_t rslt = 0;
/* FOC configuration structure */
struct bmi160_foc_conf foc_conf;
/* Structure to store the offsets */
struct bmi160_offsets offsets;
/* Enable offset update for accel */
foc_conf.acc_off_en = BMI160_ENABLE;
/* Enable offset update for gyro */
foc_conf.gyro_off_en = BMI160_ENABLE;
/* offset values set by user */
offsets.off_acc_x = 0x10;
offsets.off_acc_y = 0x10;
offsets.off_acc_z = 0x10;
offsets.off_gyro_x = 0x10;
offsets.off_gyro_y = 0x10;
offsets.off_gyro_z = 0x10;
rslt = bmi160_set_offsets(&foc_conf, &offsets, sen);
/* After offset setting the data read from the
* sensor will have the corresponding offset */
return rslt;
}
```
#### Example for updating the offsets into NVM
> The offsets set by this method will be present in NVM and will be
> restored on POR/soft-reset
```
/* An example for updating manual offsets to sensor */
int8_t write_offsets_nvm(struct bmi160_dev *dev)
{
int8_t rslt = 0;
/* FOC configuration structure */
struct bmi160_foc_conf foc_conf;
/* Structure to store the offsets */
struct bmi160_offsets offsets;
/* Enable offset update for accel */
foc_conf.acc_off_en = BMI160_ENABLE;
/* Enable offset update for gyro */
foc_conf.gyro_off_en = BMI160_ENABLE;
/* offset values set by user as per their reference
* Resolution of accel = 3.9mg/LSB
* Resolution of gyro = (0.061degrees/second)/LSB */
offsets.off_acc_x = 10;
offsets.off_acc_y = -15;
offsets.off_acc_z = 20;
offsets.off_gyro_x = 30;
offsets.off_gyro_y = -35;
offsets.off_gyro_z = -40;
rslt = bmi160_set_offsets(&foc_conf, &offsets, sen);
if (rslt == BMI160_OK) {
/* Update the NVM */
rslt = bmi160_update_nvm(dev);
}
/* After this procedure the offsets are written to
* NVM and restored on POR/soft-reset
* The set values can be removed to ideal case by
* invoking the following APIs
* - bmi160_start_foc()
* - bmi160_update_nvm()
*/
return rslt;
}
```
## Copyright (C) 2019 Bosch Sensortec GmbH

6336
imu/BMI160_driver/bmi160.c Normal file
View File

File diff suppressed because it is too large Load Diff

693
imu/BMI160_driver/bmi160.h Normal file
View File

@ -0,0 +1,693 @@
/**
* Copyright (C) 2018 - 2019 Bosch Sensortec GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* Neither the name of the copyright holder nor the names of the
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
* OR CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
* OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
*
* The information provided is believed to be accurate and reliable.
* The copyright holder assumes no responsibility
* for the consequences of use
* of such information nor for any infringement of patents or
* other rights of third parties which may result from its use.
* No license is granted by implication or otherwise under any patent or
* patent rights of the copyright holder.
*
* @file bmi160.h
* @date 13 Mar 2019
* @version 3.7.7
* @brief
*
*/
/*!
* @defgroup bmi160
* @brief
* @{*/
#ifndef BMI160_H_
#define BMI160_H_
/*************************** C++ guard macro *****************************/
#ifdef __cplusplus
extern "C" {
#endif
#include "bmi160_defs.h"
#ifdef __KERNEL__
#include <bmi160_math.h>
#else
#include <math.h>
#include <string.h>
#include <stdlib.h>
#endif
/*********************** User function prototypes ************************/
/*!
* @brief This API is the entry point for sensor.It performs
* the selection of I2C/SPI read mechanism according to the
* selected interface and reads the chip-id of bmi160 sensor.
*
* @param[in,out] dev : Structure instance of bmi160_dev
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_init(struct bmi160_dev *dev);
/*!
* @brief This API reads the data from the given register address of sensor.
*
* @param[in] reg_addr : Register address from where the data to be read
* @param[out] data : Pointer to data buffer to store the read data.
* @param[in] len : No of bytes of data to be read.
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note For most of the registers auto address increment applies, with the
* exception of a few special registers, which trap the address. For e.g.,
* Register address - 0x24(BMI160_FIFO_DATA_ADDR)
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
/*!
* @brief This API writes the given data to the register address
* of sensor.
*
* @param[in] reg_addr : Register address from where the data to be written.
* @param[in] data : Pointer to data buffer which is to be written
* in the sensor.
* @param[in] len : No of bytes of data to write..
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
/*!
* @brief This API resets and restarts the device.
* All register values are overwritten with default parameters.
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error.
*/
int8_t bmi160_soft_reset(struct bmi160_dev *dev);
/*!
* @brief This API configures the power mode, range and bandwidth
* of sensor.
*
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error.
*/
int8_t bmi160_set_sens_conf(struct bmi160_dev *dev);
/*!
* @brief This API sets the power mode of the sensor.
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error.
*/
int8_t bmi160_set_power_mode(struct bmi160_dev *dev);
/*!
* @brief This API gets the power mode of the sensor.
*
* @param[in] power_mode : Power mode of the sensor
* @param[in] dev : Structure instance of bmi160_dev
*
* power_mode Macros possible values for pmu_status->aux_pmu_status :
* - BMI160_AUX_PMU_SUSPEND
* - BMI160_AUX_PMU_NORMAL
* - BMI160_AUX_PMU_LOW_POWER
*
* power_mode Macros possible values for pmu_status->gyro_pmu_status :
* - BMI160_GYRO_PMU_SUSPEND
* - BMI160_GYRO_PMU_NORMAL
* - BMI160_GYRO_PMU_FSU
*
* power_mode Macros possible values for pmu_status->accel_pmu_status :
* - BMI160_ACCEL_PMU_SUSPEND
* - BMI160_ACCEL_PMU_NORMAL
* - BMI160_ACCEL_PMU_LOW_POWER
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error.
*/
int8_t bmi160_get_power_mode(struct bmi160_pmu_status *pmu_status, const struct bmi160_dev *dev);
/*!
* @brief This API reads sensor data, stores it in
* the bmi160_sensor_data structure pointer passed by the user.
* The user can ask for accel data ,gyro data or both sensor
* data using bmi160_select_sensor enum
*
* @param[in] select_sensor : enum to choose accel,gyro or both sensor data
* @param[out] accel : Structure pointer to store accel data
* @param[out] gyro : Structure pointer to store gyro data
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_get_sensor_data(uint8_t select_sensor,
struct bmi160_sensor_data *accel,
struct bmi160_sensor_data *gyro,
const struct bmi160_dev *dev);
/*!
* @brief This API configures the necessary interrupt based on
* the user settings in the bmi160_int_settg structure instance.
*
* @param[in] int_config : Structure instance of bmi160_int_settg.
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev);
/*!
* @brief This API enables the step counter feature.
*
* @param[in] step_cnt_enable : value to enable or disable
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev);
/*!
* @brief This API reads the step counter value.
*
* @param[in] step_val : Pointer to store the step counter value.
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev);
/*!
* @brief This API reads the mention no of byte of data from the given
* register address of auxiliary sensor.
*
* @param[in] reg_addr : Address of register to read.
* @param[in] aux_data : Pointer to store the read data.
* @param[in] len : No of bytes to read.
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
/*!
* @brief This API writes the mention no of byte of data to the given
* register address of auxiliary sensor.
*
* @param[in] reg_addr : Address of register to write.
* @param[in] aux_data : Pointer to write data.
* @param[in] len : No of bytes to write.
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
/*!
* @brief This API initialize the auxiliary sensor
* in order to access it.
*
* @param[in] dev : Structure instance of bmi160_dev.
* @note : Refer user guide for detailed info.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_aux_init(const struct bmi160_dev *dev);
/*!
* @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode
* Thus enabling the auto update of 8 bytes of data from auxiliary sensor
* to BMI160 register address 0x04 to 0x0B
*
* @param[in] data_addr : Starting address of aux. sensor's data register
* (BMI160 registers 0x04 to 0x0B will be updated
* with 8 bytes of data from auxiliary sensor
* starting from this register address.)
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note : Set the value of auxiliary polling rate by setting
* dev->aux_cfg.aux_odr to the required value from the table
* before calling this API
*
* dev->aux_cfg.aux_odr | Auxiliary ODR (Hz)
* -----------------------|-----------------------
* BMI160_AUX_ODR_0_78HZ | 25/32
* BMI160_AUX_ODR_1_56HZ | 25/16
* BMI160_AUX_ODR_3_12HZ | 25/8
* BMI160_AUX_ODR_6_25HZ | 25/4
* BMI160_AUX_ODR_12_5HZ | 25/2
* BMI160_AUX_ODR_25HZ | 25
* BMI160_AUX_ODR_50HZ | 50
* BMI160_AUX_ODR_100HZ | 100
* BMI160_AUX_ODR_200HZ | 200
* BMI160_AUX_ODR_400HZ | 400
* BMI160_AUX_ODR_800HZ | 800
*
* @note : Other values of dev->aux_cfg.aux_odr are reserved and not for use
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev);
/*!
* @brief This API configures the 0x4C register and settings like
* Auxiliary sensor manual enable/ disable and aux burst read length.
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev);
/*!
* @brief This API is used to read the raw uncompensated auxiliary sensor
* data of 8 bytes from BMI160 register address 0x04 to 0x0B
*
* @param[in] aux_data : Pointer to user array of length 8 bytes
* Ensure that the aux_data array is of
* length 8 bytes
* @param[in] dev : Structure instance of bmi160_dev
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*/
int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev);
/*!
* @brief This is used to perform self test of accel/gyro of the BMI160 sensor
*
* @param[in] select_sensor : enum to choose accel or gyro for self test
* @param[in] dev : Structure instance of bmi160_dev
*
* @note self test can be performed either for accel/gyro at any instant.
*
* value of select_sensor | Inference
*----------------------------------|--------------------------------
* BMI160_ACCEL_ONLY | Accel self test enabled
* BMI160_GYRO_ONLY | Gyro self test enabled
* BMI160_BOTH_ACCEL_AND_GYRO | NOT TO BE USED
*
* @note The return value of this API gives us the result of self test.
*
* @note Performing self test does soft reset of the sensor, User can
* set the desired settings after performing the self test.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error / +ve value -> Self-test fail
*
* Return value | Result of self test
* --------------------------------|---------------------------------
* BMI160_OK | Self test success
* BMI160_W_GYRO_SELF_TEST_FAIL | Gyro self test fail
* BMI160_W_ACCEl_SELF_TEST_FAIL | Accel self test fail
*/
int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev);
/*!
* @brief This API reads data from the fifo buffer.
*
* @note User has to allocate the FIFO buffer along with
* corresponding fifo length from his side before calling this API
* as mentioned in the readme.md
*
* @note User must specify the number of bytes to read from the FIFO in
* dev->fifo->length , It will be updated by the number of bytes actually
* read from FIFO after calling this API
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval zero -> Success / -ve value -> Error
*
*/
int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev);
/*!
* @brief This API writes fifo_flush command to command register.This
* action clears all data in the Fifo without changing fifo configuration
* settings.
*
* @param[in] dev : Structure instance of bmi160_dev
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev);
/*! @brief This API sets the FIFO configuration in the sensor.
*
* @param[in] config : variable used to specify the FIFO
* configurations which are to be enabled or disabled in the sensor.
*
* @note : User can set either set one or more or all FIFO configurations
* by ORing the below mentioned macros.
* config | Value
* ------------------------|---------------------------
* BMI160_FIFO_TIME | 0x02
* BMI160_FIFO_TAG_INT2 | 0x04
* BMI160_FIFO_TAG_INT1 | 0x08
* BMI160_FIFO_HEADER | 0x10
* BMI160_FIFO_AUX | 0x20
* BMI160_FIFO_ACCEL | 0x40
* BMI160_FIFO_GYRO | 0x80
*
* @param[in] enable : Parameter used to enable or disable the above
* FIFO configuration
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return status of bus communication result
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev);
/*! @brief This API is used to configure the down sampling ratios of
* the accel and gyro data for FIFO.Also, it configures filtered or
* pre-filtered data for the fifo for accel and gyro.
*
* @param[in] fifo_down : variable used to specify the FIFO down
* configurations which are to be enabled or disabled in the sensor.
*
* @note The user must select one among the following macros to
* select down-sampling ratio for accel
* config | Value
* -------------------------------------|---------------------------
* BMI160_ACCEL_FIFO_DOWN_ZERO | 0x00
* BMI160_ACCEL_FIFO_DOWN_ONE | 0x10
* BMI160_ACCEL_FIFO_DOWN_TWO | 0x20
* BMI160_ACCEL_FIFO_DOWN_THREE | 0x30
* BMI160_ACCEL_FIFO_DOWN_FOUR | 0x40
* BMI160_ACCEL_FIFO_DOWN_FIVE | 0x50
* BMI160_ACCEL_FIFO_DOWN_SIX | 0x60
* BMI160_ACCEL_FIFO_DOWN_SEVEN | 0x70
*
* @note The user must select one among the following macros to
* select down-sampling ratio for gyro
*
* config | Value
* -------------------------------------|---------------------------
* BMI160_GYRO_FIFO_DOWN_ZERO | 0x00
* BMI160_GYRO_FIFO_DOWN_ONE | 0x01
* BMI160_GYRO_FIFO_DOWN_TWO | 0x02
* BMI160_GYRO_FIFO_DOWN_THREE | 0x03
* BMI160_GYRO_FIFO_DOWN_FOUR | 0x04
* BMI160_GYRO_FIFO_DOWN_FIVE | 0x05
* BMI160_GYRO_FIFO_DOWN_SIX | 0x06
* BMI160_GYRO_FIFO_DOWN_SEVEN | 0x07
*
* @note The user can enable filtered accel data by the following macro
* config | Value
* -------------------------------------|---------------------------
* BMI160_ACCEL_FIFO_FILT_EN | 0x80
*
* @note The user can enable filtered gyro data by the following macro
* config | Value
* -------------------------------------|---------------------------
* BMI160_GYRO_FIFO_FILT_EN | 0x08
*
* @note : By ORing the above mentioned macros, the user can select
* the required FIFO down config settings
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return status of bus communication result
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev);
/*!
* @brief This API sets the FIFO watermark level in the sensor.
*
* @note The FIFO watermark is issued when the FIFO fill level is
* equal or above the watermark level and units of watermark is 4 bytes.
*
* @param[in] fifo_wm : Variable used to set the FIFO water mark level
* @param[in] dev : Structure instance of bmi160_dev
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev);
/*!
* @brief This API parses and extracts the accelerometer frames from
* FIFO data read by the "bmi160_get_fifo_data" API and stores it in
* the "accel_data" structure instance.
*
* @note The bmi160_extract_accel API should be called only after
* reading the FIFO data by calling the bmi160_get_fifo_data() API.
*
* @param[out] accel_data : Structure instance of bmi160_sensor_data
* where the accelerometer data in FIFO is stored.
* @param[in,out] accel_length : Number of valid accelerometer frames
* (x,y,z axes data) read out from fifo.
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note accel_length is updated with the number of valid accelerometer
* frames extracted from fifo (1 accel frame = 6 bytes) at the end of
* execution of this API.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev);
/*!
* @brief This API parses and extracts the gyro frames from
* FIFO data read by the "bmi160_get_fifo_data" API and stores it in
* the "gyro_data" structure instance.
*
* @note The bmi160_extract_gyro API should be called only after
* reading the FIFO data by calling the bmi160_get_fifo_data() API.
*
* @param[out] gyro_data : Structure instance of bmi160_sensor_data
* where the gyro data in FIFO is stored.
* @param[in,out] gyro_length : Number of valid gyro frames
* (x,y,z axes data) read out from fifo.
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note gyro_length is updated with the number of valid gyro
* frames extracted from fifo (1 gyro frame = 6 bytes) at the end of
* execution of this API.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev);
/*!
* @brief This API parses and extracts the aux frames from
* FIFO data read by the "bmi160_get_fifo_data" API and stores it in
* the bmi160_aux_data structure instance.
*
* @note The bmi160_extract_aux API should be called only after
* reading the FIFO data by calling the bmi160_get_fifo_data() API.
*
* @param[out] aux_data : Structure instance of bmi160_aux_data
* where the aux data in FIFO is stored.
* @param[in,out] aux_len : Number of valid aux frames (8bytes)
* read out from FIFO.
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note aux_len is updated with the number of valid aux
* frames extracted from fifo (1 aux frame = 8 bytes) at the end of
* execution of this API.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*
*/
int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev);
/*!
* @brief This API starts the FOC of accel and gyro
*
* @note FOC should not be used in low-power mode of sensor
*
* @note Accel FOC targets values of +1g , 0g , -1g
* Gyro FOC always targets value of 0 dps
*
* @param[in] foc_conf : Structure instance of bmi160_foc_conf which
* has the FOC configuration
* @param[in,out] offset : Structure instance to store Offset
* values read from sensor
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note Pre-requisites for triggering FOC in accel , Set the following,
* Enable the acc_off_en
* Ex : foc_conf.acc_off_en = BMI160_ENABLE;
*
* Set the desired target values of FOC to each axes (x,y,z) by using the
* following macros
* - BMI160_FOC_ACCEL_DISABLED
* - BMI160_FOC_ACCEL_POSITIVE_G
* - BMI160_FOC_ACCEL_NEGATIVE_G
* - BMI160_FOC_ACCEL_0G
*
* Ex : foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G;
* foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G;
* foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G;
*
* @note Pre-requisites for triggering FOC in gyro ,
* Set the following parameters,
*
* Ex : foc_conf.foc_gyr_en = BMI160_ENABLE;
* foc_conf.gyro_off_en = BMI160_ENABLE;
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*/
int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf,
struct bmi160_offsets *offset,
struct bmi160_dev const *dev);
/*!
* @brief This API reads and stores the offset values of accel and gyro
*
* @param[in,out] offset : Structure instance of bmi160_offsets in which
* the offset values are read and stored
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*/
int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev);
/*!
* @brief This API writes the offset values of accel and gyro to
* the sensor but these values will be reset on POR or soft reset.
*
* @param[in] foc_conf : Structure instance of bmi160_foc_conf which
* has the FOC configuration
* @param[in] offset : Structure instance in which user updates offset
* values which are to be written in the sensor
* @param[in] dev : Structure instance of bmi160_dev.
*
* @note Offsets can be set by user like offset->off_acc_x = 10;
* where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second
*
* @note BMI160 offset values for xyz axes of accel should be within range of
* BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127)
*
* @note BMI160 offset values for xyz axes of gyro should be within range of
* BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511)
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*/
int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf,
const struct bmi160_offsets *offset,
struct bmi160_dev const *dev);
/*!
* @brief This API writes the image registers values to NVM which is
* stored even after POR or soft reset
*
* @param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*/
int8_t bmi160_update_nvm(struct bmi160_dev const *dev);
/*!
* @brief This API gets the interrupt status from the sensor.
*
* @param[in] int_status_sel : Enum variable to select either individual or all the
* interrupt status bits.
* @param[in] int_status : pointer variable to get the interrupt status
* from the sensor.
* param[in] dev : Structure instance of bmi160_dev.
*
* @return Result of API execution status
* @retval 0 -> Success
* @retval Any non zero value -> Fail
*/
int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel,
union bmi160_int_status *int_status,
struct bmi160_dev const *dev);
/*************************** C++ guard macro *****************************/
#ifdef __cplusplus
}
#endif
#endif /* BMI160_H_ */
/** @}*/

1674
imu/BMI160_driver/bmi160_defs.h Normal file
View File

File diff suppressed because it is too large Load Diff

69
imu/ahrs.c
View File

@ -18,11 +18,11 @@
#include "utils.h"
#include <math.h>
// Settings
static float mahonyKp = (2.0f * 0.3f); // 2 * proportional gain
static float mahonyKi = (2.0f * 0.0f); // 2 * integral gain
static float madgwickAccConfidenceDecay = (1.0);
static float madgwickBeta = (0.5f); // 2 * proportional gain??
// Private variables
static float m_acc_confidence_decay = 1.0;
static float m_kp = 0.3;
static float m_ki = 0.0;
static float m_beta = 0.1;
// Private functions
static float invSqrt(float x);
@ -37,7 +37,7 @@ static float calculateAccConfidence(float accMag, float *accMagP) {
accMag = *accMagP * 0.9f + accMag * 0.1f;
*accMagP = accMag;
confidence = 1.0 - (madgwickAccConfidenceDecay * sqrtf(fabsf(accMag - 1.0f)));
confidence = 1.0 - (m_acc_confidence_decay * sqrtf(fabsf(accMag - 1.0f)));
utils_truncate_number(&confidence, 0.0, 1.0);
return confidence;
@ -55,6 +55,13 @@ void ahrs_init_attitude_info(ATTITUDE_INFO *att) {
att->initialUpdateDone = 0;
}
void ahrs_update_all_parameters(float confidence_decay, float kp, float ki, float beta) {
m_acc_confidence_decay = confidence_decay;
m_kp = kp;
m_ki = ki;
m_beta = beta;
}
void ahrs_update_initial_orientation(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att) {
// See https://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf
// and http://sedris.org/wg8home/Documents/WG80485.pdf
@ -131,8 +138,8 @@ void ahrs_update_mahony(float *gyroXYZ, float *accelXYZ, float *magXYZ, float dt
float halfex, halfey, halfez;
float accelConfidence;
volatile float twoKp = mahonyKp;
volatile float twoKi = mahonyKi;
volatile float twoKp = 2.0 * m_kp;
volatile float twoKi = 2.0 * m_ki;
accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP);
twoKp *= accelConfidence;
@ -242,8 +249,8 @@ void ahrs_update_mahony_imu(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_
float halfex, halfey, halfez;
float accelConfidence;
volatile float twoKp = mahonyKp;
volatile float twoKi = mahonyKi;
volatile float twoKp = 2.0 * m_kp;
volatile float twoKi = 2.0 * m_ki;
accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP);
twoKp *= accelConfidence;
@ -405,10 +412,10 @@ void ahrs_update_madgwick(float *gyroXYZ, float *accelXYZ, float *magXYZ, float
// Apply feedback step
accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP);
qDot1 -= madgwickBeta * s0 * accelConfidence;
qDot2 -= madgwickBeta * s1 * accelConfidence;
qDot3 -= madgwickBeta * s2 * accelConfidence;
qDot4 -= madgwickBeta * s3 * accelConfidence;
qDot1 -= m_beta * s0 * accelConfidence;
qDot2 -= m_beta * s1 * accelConfidence;
qDot3 -= m_beta * s2 * accelConfidence;
qDot4 -= m_beta * s3 * accelConfidence;
}
// Integrate rate of change of quaternion to yield quaternion
@ -496,10 +503,10 @@ void ahrs_update_madgwick_imu(float *gyroXYZ, float *accelXYZ, float dt, ATTITUD
// Apply feedback step
accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP);
qDot1 -= madgwickBeta * s0 * accelConfidence;
qDot2 -= madgwickBeta * s1 * accelConfidence;
qDot3 -= madgwickBeta * s2 * accelConfidence;
qDot4 -= madgwickBeta * s3 * accelConfidence;
qDot1 -= m_beta * s0 * accelConfidence;
qDot2 -= m_beta * s1 * accelConfidence;
qDot3 -= m_beta * s2 * accelConfidence;
qDot4 -= m_beta * s3 * accelConfidence;
}
// Integrate rate of change of quaternion to yield quaternion
@ -560,32 +567,6 @@ void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att) {
rpy[2] = -atan2f(q0 * q3 + q1 * q2, 0.5 - (q2 * q2 + q3 * q3));
}
float ahrs_get_mahony_kp(){
return mahonyKp;
}
float ahrs_get_mahony_ki(){
return mahonyKi;
}
float ahrs_get_madgwick_acc_confidence_decay(){
return madgwickAccConfidenceDecay;
}
float ahrs_get_madgwick_beta(){
return madgwickBeta;
}
void ahrs_set_mahony_kp(float kp){
mahonyKp = kp;
}
void ahrs_set_mahony_ki(float ki){
mahonyKi = ki;
}
void ahrs_set_madgwick_acc_confidence_decay(float accConfidenceDecay){
madgwickAccConfidenceDecay = accConfidenceDecay;
}
void ahrs_set_madgwick_beta(float beta){
madgwickBeta = beta;
}
static float invSqrt(float x) {
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

11
imu/ahrs.h
View File

@ -17,6 +17,7 @@
// Function declarations
void ahrs_init_attitude_info(ATTITUDE_INFO *att);
void ahrs_update_all_parameters(float confidence_decay, float kp, float ki, float beta);
void ahrs_update_initial_orientation(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att);
void ahrs_update_mahony(float *gyroXYZ, float *accelXYZ, float *magXYZ, float dt, ATTITUDE_INFO *att);
@ -30,14 +31,4 @@ float ahrs_get_pitch(ATTITUDE_INFO *att);
float ahrs_get_yaw(ATTITUDE_INFO *att);
void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att);
float ahrs_get_mahony_kp(void);
float ahrs_get_mahony_ki(void);
float ahrs_get_madgwick_acc_confidence_decay(void);
float ahrs_get_madgwick_beta(void);
void ahrs_set_mahony_kp(float mahony_kp);
void ahrs_set_mahony_ki(float mahony_ki);
void ahrs_set_madgwick_acc_confidence_decay(float madgwick_acc_confidence_decay);
void ahrs_set_madgwick_beta(float madgwick_beta);
#endif

118
imu/bmi160_wrapper.c Normal file
View File

@ -0,0 +1,118 @@
/*
Copyright 2019 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/>.
*/
#include "bmi160_wrapper.h"
#include <stdio.h>
#include <string.h>
// Threads
static THD_FUNCTION(bmi_thread, arg);
// Private functions
static bool reset_init_bmi(BMI_STATE *s);
void user_delay_ms(uint32_t ms);
void bmi160_wrapper_init(BMI_STATE *s, stkalign_t *work_area, size_t work_area_size) {
s->read_callback = 0;
if (reset_init_bmi(s)) {
s->should_stop = false;
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, bmi_thread, s);
}
}
void bmi160_wrapper_set_read_callback(BMI_STATE *s, void(*func)(float *accel, float *gyro, float *mag)) {
s->read_callback = func;
}
void bmi160_wrapper_stop(BMI_STATE *s) {
s->should_stop = true;
while(s->is_running) {
chThdSleep(1);
}
}
static bool reset_init_bmi(BMI_STATE *s) {
s->sensor.delay_ms = user_delay_ms;
bmi160_init(&(s->sensor));
s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ;
s->sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G;
s->sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
s->sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ;
s->sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
s->sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
s->sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
int8_t res = bmi160_set_sens_conf(&(s->sensor));
return res == BMI160_OK;
}
void user_delay_ms(uint32_t ms) {
chThdSleepMilliseconds(ms);
}
static THD_FUNCTION(bmi_thread, arg) {
BMI_STATE *s = (BMI_STATE*)arg;
chRegSetThreadName("BMI Sampling");
s->is_running = true;
for(;;) {
struct bmi160_sensor_data accel;
struct bmi160_sensor_data gyro;
int8_t res = bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL),
&accel, &gyro, &(s->sensor));
if (res != BMI160_OK) {
chThdSleepMilliseconds(5);
continue;
}
float tmp_accel[3], tmp_gyro[3], tmp_mag[3];
tmp_accel[0] = (float)accel.x * 16.0 / 32768.0;
tmp_accel[1] = (float)accel.y * 16.0 / 32768.0;
tmp_accel[2] = (float)accel.z * 16.0 / 32768.0;
tmp_gyro[0] = (float)gyro.x * 2000.0 / 32768.0;
tmp_gyro[1] = (float)gyro.y * 2000.0 / 32768.0;
tmp_gyro[2] = (float)gyro.z * 2000.0 / 32768.0;
memset(tmp_mag, 0, sizeof(tmp_mag));
if (s->read_callback) {
s->read_callback(tmp_accel, tmp_gyro, tmp_mag);
}
if (s->should_stop) {
s->is_running = false;
return;
}
chThdSleepMicroseconds(1000000 / s->rate_hz);
}
}

44
imu/bmi160_wrapper.h Normal file
View File

@ -0,0 +1,44 @@
/*
Copyright 2019 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 IMU_BMI160_WRAPPER_H_
#define IMU_BMI160_WRAPPER_H_
#include "ch.h"
#include "hal.h"
#include <stdint.h>
#include <stdbool.h>
#include "i2c_bb.h"
#include "bmi160.h"
typedef struct {
void(*read_callback)(float *accel, float *gyro, float *mag);
struct bmi160_dev sensor;
volatile bool is_running;
volatile bool should_stop;
int rate_hz;
} BMI_STATE;
void bmi160_wrapper_init(BMI_STATE *s, stkalign_t *work_area, size_t work_area_size);
void bmi160_wrapper_set_read_callback(BMI_STATE *s, void(*func)(float *accel, float *gyro, float *mag));
void bmi160_wrapper_stop(BMI_STATE *s);
#endif /* IMU_BMI160_WRAPPER_H_ */

22
imu/icm20948.c
View File

@ -45,6 +45,7 @@ void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
s->read_callback = 0;
if (reset_init_icm(s)) {
s->should_stop = false;
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, icm_thread, s);
}
@ -63,6 +64,18 @@ void icm20948_set_read_callback(ICM20948_STATE *s, void(*func)(float *accel, flo
s->read_callback = func;
}
void icm20948_stop(ICM20948_STATE *s) {
s->should_stop = true;
while(s->is_running) {
chThdSleep(1);
}
if (s == m_terminal_state) {
terminal_unregister_callback(terminal_read_reg);
m_terminal_state = 0;
}
}
static void terminal_read_reg(int argc, const char **argv) {
if (argc == 3) {
int bank = -1;
@ -146,6 +159,8 @@ static THD_FUNCTION(icm_thread, arg) {
chRegSetThreadName("ICM Sampling");
s->is_running = true;
for(;;) {
uint8_t txb[1];
uint8_t rxb[12];
@ -175,6 +190,11 @@ static THD_FUNCTION(icm_thread, arg) {
chThdSleepMilliseconds(10);
}
chThdSleepMilliseconds(5);
if (s->should_stop) {
s->is_running = false;
return;
}
chThdSleepMicroseconds(1000000 / s->rate_hz);
}
}

4
imu/icm20948.h
View File

@ -32,11 +32,15 @@ typedef struct {
i2c_bb_state *i2cs;
uint8_t i2c_address;
void(*read_callback)(float *accel, float *gyro, float *mag);
volatile bool is_running;
volatile bool should_stop;
int rate_hz;
} ICM20948_STATE;
void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
stkalign_t *work_area, size_t work_area_size);
void icm20948_set_read_callback(ICM20948_STATE *s, void(*func)(float *accel, float *gyro, float *mag));
void icm20948_stop(ICM20948_STATE *s);
// All banks
#define ICM20948_BANK_SEL 0x7F

179
imu/imu.c
View File

@ -25,6 +25,8 @@
#include "terminal.h"
#include "commands.h"
#include "icm20948.h"
#include "bmi160_wrapper.h"
#include "utils.h"
#include <math.h>
#include <string.h>
@ -35,35 +37,61 @@ static float m_accel[3], m_gyro[3], m_mag[3];
static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_t)];
static i2c_bb_state m_i2c_bb;
static ICM20948_STATE m_icm20948_state;
static BMI_STATE m_bmi_state;
static imu_config m_settings;
static float m_gyro_offset[3] = {0.0};
// Private functions
static void imu_read_callback(float *accel, float *gyro, float *mag);
static void terminal_rpy(int argc, const char **argv);
static void terminal_gyro_info(int argc, const char **argv);
int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
void imu_init(bool use_peripheral) {
void imu_init(imu_config *set) {
m_settings = *set;
memset(m_gyro_offset, 0, sizeof(m_gyro_offset));
imu_stop();
ahrs_update_all_parameters(set->accel_confidence_decay, set->mahony_kp,
set->mahony_ki, set->madgwick_beta);
ahrs_init_attitude_info(&m_att);
if(use_peripheral){
imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
} else {
mpu9150_set_rate_hz(set->sample_rate_hz);
m_icm20948_state.rate_hz = set->sample_rate_hz;
m_bmi_state.rate_hz = set->sample_rate_hz;
if (set->type == IMU_TYPE_INTERNAL) {
#ifdef MPU9X50_SDA_GPIO
imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN,
MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN);
MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN);
#endif
#ifdef ICM20948_SDA_GPIO
imu_init_icm20948(ICM20948_SDA_GPIO, ICM20948_SDA_PIN,
ICM20948_SCL_GPIO, ICM20948_SCL_PIN, ICM20948_AD0_VAL);
ICM20948_SCL_GPIO, ICM20948_SCL_PIN, ICM20948_AD0_VAL);
#endif
#ifdef BMI160_SDA_GPIO
imu_init_bmi160(BMI160_SDA_GPIO, BMI160_SDA_PIN,
BMI160_SCL_GPIO, BMI160_SCL_PIN);
#endif
} else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) {
imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
} else if (set->type == IMU_TYPE_EXTERNAL_ICM20948) {
imu_init_icm20948(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, 0);
} else if (set->type == IMU_TYPE_EXTERNAL_BMI160) {
imu_init_bmi160(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
}
terminal_register_command_callback(
"imu_rpy",
"Print 100 roll/pitch/yaw samples at 10 Hz",
"imu_gyro_info",
"Print gyro offsets",
0,
terminal_rpy);
terminal_gyro_info);
}
i2c_bb_state *imu_get_i2c(void) {
@ -72,6 +100,7 @@ i2c_bb_state *imu_get_i2c(void) {
void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin) {
imu_stop();
mpu9150_init(sda_gpio, sda_pin,
scl_gpio, scl_pin,
@ -81,6 +110,7 @@ void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin, int ad0_val) {
imu_stop();
m_i2c_bb.sda_gpio = sda_gpio;
m_i2c_bb.sda_pin = sda_pin;
@ -94,6 +124,31 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
icm20948_set_read_callback(&m_icm20948_state, imu_read_callback);
}
void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin) {
imu_stop();
m_i2c_bb.sda_gpio = sda_gpio;
m_i2c_bb.sda_pin = sda_pin;
m_i2c_bb.scl_gpio = scl_gpio;
m_i2c_bb.scl_pin = scl_pin;
i2c_bb_init(&m_i2c_bb);
m_bmi_state.sensor.id = BMI160_I2C_ADDR;
m_bmi_state.sensor.interface = BMI160_I2C_INTF;
m_bmi_state.sensor.read = user_i2c_read;
m_bmi_state.sensor.write = user_i2c_write;
bmi160_wrapper_init(&m_bmi_state, m_thd_work_area, sizeof(m_thd_work_area));
bmi160_wrapper_set_read_callback(&m_bmi_state, imu_read_callback);
}
void imu_stop(void) {
mpu9150_stop();
icm20948_stop(&m_icm20948_state);
bmi160_wrapper_stop(&m_bmi_state);
}
float imu_get_roll(void) {
return ahrs_get_roll(&m_att);
}
@ -161,31 +216,54 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
last_time = timer_time_now();
#ifdef IMU_FLIP
m_accel[0] = -accel[0];
m_accel[1] = accel[1];
m_accel[2] = -accel[2];
m_gyro[0] = -gyro[0];
m_gyro[1] = gyro[1];
m_gyro[2] = -gyro[2];
m_mag[0] = -mag[0];
m_mag[1] = mag[1];
m_mag[2] = -mag[2];
#else
m_accel[0] = accel[0];
m_accel[1] = accel[1];
m_accel[2] = accel[2];
m_gyro[0] = gyro[0];
m_gyro[1] = gyro[1];
m_gyro[2] = gyro[2];
m_mag[0] = mag[0];
m_mag[1] = mag[1];
m_mag[2] = mag[2];
accel[0] *= -1.0;
accel[2] *= -1.0;
gyro[0] *= -1.0;
gyro[2] *= -1.0;
mag[0] *= -1.0;
mag[2] *= -1.0;
#endif
// Rotate axes (ZYX)
float s1 = sinf(m_settings.rot_yaw * M_PI / 180.0);
float c1 = cosf(m_settings.rot_yaw * M_PI / 180.0);
float s2 = sinf(m_settings.rot_pitch * M_PI / 180.0);
float c2 = cosf(m_settings.rot_pitch * M_PI / 180.0);
float s3 = sinf(m_settings.rot_roll * M_PI / 180.0);
float c3 = cosf(m_settings.rot_roll * M_PI / 180.0);
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3;
m_accel[0] = accel[0] * m11 + accel[1] * m12 + accel[2] * m13;
m_accel[1] = accel[0] * m21 + accel[1] * m22 + accel[2] * m23;
m_accel[2] = accel[0] * m31 + accel[1] * m32 + accel[2] * m33;
m_gyro[0] = gyro[0] * m11 + gyro[1] * m12 + gyro[2] * m13;
m_gyro[1] = gyro[0] * m21 + gyro[1] * m22 + gyro[2] * m23;
m_gyro[2] = gyro[0] * m31 + gyro[1] * m32 + gyro[2] * m33;
m_mag[0] = mag[0] * m11 + mag[1] * m12 + mag[2] * m13;
m_mag[1] = mag[0] * m21 + mag[1] * m22 + mag[2] * m23;
m_mag[2] = mag[0] * m31 + mag[1] * m32 + mag[2] * m33;
// Accelerometer and Gyro offset compensation and estimation
for (int i = 0;i < 3;i++) {
m_accel[i] -= m_settings.accel_offsets[i];
m_gyro[i] -= m_settings.gyro_offsets[i];
if (m_settings.gyro_offset_comp_fact[i] > 0.0) {
utils_step_towards(&m_gyro_offset[i], m_gyro[i], m_settings.gyro_offset_comp_fact[i] * dt);
utils_truncate_number_abs(&m_gyro_offset[i], m_settings.gyro_offset_comp_clamp);
} else {
m_gyro_offset[i] = 0.0;
}
m_gyro[i] -= m_gyro_offset[i];
}
float gyro_rad[3];
gyro_rad[0] = m_gyro[0] * M_PI / 180.0;
gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
@ -194,18 +272,29 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
}
static void terminal_rpy(int argc, const char **argv) {
static void terminal_gyro_info(int argc, const char **argv) {
(void)argc;
(void)argv;
for (int i = 0;i < 100;i++) {
commands_printf("R: %.2f P: %.2f Y: %.2f",
(double)(imu_get_roll() * 180.0 / M_PI),
(double)(imu_get_pitch() * 180.0 / M_PI),
(double)(imu_get_yaw() * 180.0 / M_PI));
chThdSleepMilliseconds(100);
}
commands_printf(" ");
commands_printf("Gyro offsets: [%.3f %.3f %.3f]\n",
(double)(m_settings.gyro_offsets[0] + m_gyro_offset[0]),
(double)(m_settings.gyro_offsets[1] + m_gyro_offset[1]),
(double)(m_settings.gyro_offsets[2] + m_gyro_offset[2]));
}
int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len) {
m_i2c_bb.has_error = 0;
uint8_t txbuf[1];
txbuf[0] = reg_addr;
return i2c_bb_tx_rx(&m_i2c_bb, dev_addr, txbuf, 1, data, len) ? BMI160_OK : BMI160_E_COM_FAIL;
}
int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len) {
m_i2c_bb.has_error = 0;
uint8_t txbuf[len + 1];
txbuf[0] = reg_addr;
memcpy(txbuf + 1, data, len);
return i2c_bb_tx_rx(&m_i2c_bb, dev_addr, txbuf, len + 1, 0, 0) ? BMI160_OK : BMI160_E_COM_FAIL;
}

5
imu/imu.h
View File

@ -24,12 +24,15 @@
#include "hal.h"
#include "i2c_bb.h"
void imu_init(bool use_peripheral);
void imu_init(imu_config *set);
i2c_bb_state *imu_get_i2c(void);
void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin, int ad0_val);
void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_stop(void);
float imu_get_roll(void);
float imu_get_pitch(void);
float imu_get_yaw(void);

7
imu/imu.mk
View File

@ -1,6 +1,9 @@
IMUSRC = imu/mpu9150.c \
imu/icm20948.c \
imu/ahrs.c \
imu/imu.c
imu/imu.c \
imu/BMI160_driver/bmi160.c \
imu/bmi160_wrapper.c
IMUINC = imu
IMUINC = imu \
imu/BMI160_driver

32
imu/mpu9150.c
View File

@ -37,7 +37,6 @@
#define USE_MAGNETOMETER 1
#define MPU_I2C_TIMEOUT 10
#define MAG_DIV 10
#define ITERATION_TIME_US 5000
#define FAIL_DELAY_US 1000
#define MIN_ITERATION_DELAY_US 500
#define MAX_IDENTICAL_READS 5
@ -59,6 +58,9 @@ static volatile bool is_mpu9250;
static i2c_bb_state i2cs;
static volatile int16_t mpu9150_gyro_offsets[3];
static volatile bool mpu_found;
static volatile bool is_running;
static volatile bool should_stop;
static volatile int rate_hz = 200;
// Private functions
static int reset_init_mpu(void);
@ -87,6 +89,8 @@ void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
mag_updated = 0;
mpu_addr = MPU_ADDR1;
is_mpu9250 = 0;
is_running = false;
should_stop = false;
memset((void*)mpu9150_gyro_offsets, 0, sizeof(mpu9150_gyro_offsets));
@ -111,9 +115,10 @@ void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
terminal_read_reg);
uint8_t res = read_single_reg(MPU9150_WHO_AM_I);
if (res == 0x68 || res == 0x69 || res == 0x71) {
if (res == 0x68 || res == 0x69 || res == 0x71 || res == 0x73) {
mpu_found = true;
if (!mpu_tp) {
should_stop = false;
chThdCreateStatic(work_area, work_area_size, NORMALPRIO, mpu_thread, NULL);
}
} else {
@ -158,6 +163,16 @@ static void terminal_read_reg(int argc, const char **argv) {
}
}
void mpu9150_stop(void) {
should_stop = true;
while (is_running) {
chThdSleep(1);
}
terminal_unregister_callback(terminal_status);
terminal_unregister_callback(terminal_read_reg);
}
/**
* Determine wether this is a MPU9150 or a MPU9250.
*
@ -258,10 +273,15 @@ void mpu9150_get_accel_gyro_mag(float *accel, float *gyro, float *mag) {
mpu9150_get_mag(mag);
}
void mpu9150_set_rate_hz(int hz) {
rate_hz = hz;
}
static THD_FUNCTION(mpu_thread, arg) {
(void)arg;
chRegSetThreadName("MPU Sampling");
is_running = true;
mpu_tp = chThdGetSelfX();
static int16_t raw_accel_gyro_mag_tmp[9];
@ -274,6 +294,12 @@ static THD_FUNCTION(mpu_thread, arg) {
iteration_timer = chVTGetSystemTime();
for(;;) {
if (should_stop) {
is_running = false;
mpu_tp = 0;
return;
}
if (get_raw_accel_gyro(raw_accel_gyro_mag_tmp)) {
int is_identical = 1;
for (int i = 0;i < 6;i++) {
@ -338,7 +364,7 @@ static THD_FUNCTION(mpu_thread, arg) {
iteration_timer = chVTGetSystemTime();
}
iteration_timer += US2ST(ITERATION_TIME_US);
iteration_timer += US2ST(1000000 / rate_hz);
systime_t time_start = chVTGetSystemTime();
if (iteration_timer > time_start) {
chThdSleep(iteration_timer - time_start);

2
imu/mpu9150.h
View File

@ -27,6 +27,7 @@
void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin,
stkalign_t *work_area, size_t work_area_size);
void mpu9150_stop(void);
bool mpu9150_is_mpu9250(void);
void mpu9150_cmd_print(BaseSequentialStream *chp, int argc, char *argv[]);
void mpu9150_cmd_sample_offsets(BaseSequentialStream *chp, int argc, char *argv[]);
@ -35,6 +36,7 @@ void mpu9150_get_accel(float *accel);
void mpu9150_get_gyro(float *gyro);
void mpu9150_get_mag(float *mag);
void mpu9150_get_accel_gyro_mag(float *accel, float *gyro, float *mag);
void mpu9150_set_rate_hz(int hz);
void mpu9150_sample_gyro_offsets(uint32_t iteratons);
void mpu9150_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
uint32_t mpu9150_get_time_since_update(void);

11
main.c
View File

@ -53,6 +53,7 @@
#if HAS_BLACKMAGIC
#include "bm_if.h"
#endif
#include "shutdown.h"
/*
* HW resources used:
@ -192,7 +193,9 @@ int main(void) {
INIT_BR();
#endif
chThdSleepMilliseconds(1000);
HW_EARLY_INIT();
chThdSleepMilliseconds(100);
hw_init_gpio();
LED_RED_OFF();
@ -325,12 +328,16 @@ int main(void) {
timeout_init();
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
imu_init(false);
imu_init(&appconf.imu_conf);
#if HAS_BLACKMAGIC
bm_init();
#endif
#ifdef HW_SHUTDOWN_HOLD_ON
shutdown_init();
#endif
for(;;) {
chThdSleepMilliseconds(10);
}

121
mc_interface.c
View File

@ -34,6 +34,8 @@
#include "drv8323s.h"
#include "buffer.h"
#include "gpdrive.h"
#include "comm_can.h"
#include "shutdown.h"
#include <math.h>
#include <stdlib.h>
@ -59,6 +61,10 @@ static volatile float m_motor_id_sum;
static volatile float m_motor_iq_sum;
static volatile float m_motor_id_iterations;
static volatile float m_motor_iq_iterations;
static volatile float m_motor_vd_sum;
static volatile float m_motor_vq_sum;
static volatile float m_motor_vd_iterations;
static volatile float m_motor_vq_iterations;
static volatile float m_amp_seconds;
static volatile float m_amp_seconds_charged;
static volatile float m_watt_seconds;
@ -119,6 +125,10 @@ void mc_interface_init(mc_configuration *configuration) {
m_motor_iq_sum = 0.0;
m_motor_id_iterations = 0.0;
m_motor_iq_iterations = 0.0;
m_motor_vd_sum = 0.0;
m_motor_vq_sum = 0.0;
m_motor_vd_iterations = 0.0;
m_motor_vq_iterations = 0.0;
m_amp_seconds = 0.0;
m_amp_seconds_charged = 0.0;
m_watt_seconds = 0.0;
@ -401,6 +411,10 @@ mc_state mc_interface_get_state(void) {
}
void mc_interface_set_duty(float dutyCycle) {
if (fabsf(dutyCycle) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -421,6 +435,10 @@ void mc_interface_set_duty(float dutyCycle) {
}
void mc_interface_set_duty_noramp(float dutyCycle) {
if (fabsf(dutyCycle) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -441,6 +459,10 @@ void mc_interface_set_duty_noramp(float dutyCycle) {
}
void mc_interface_set_pid_speed(float rpm) {
if (fabsf(rpm) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -461,6 +483,8 @@ void mc_interface_set_pid_speed(float rpm) {
}
void mc_interface_set_pid_pos(float pos) {
SHUTDOWN_RESET();
if (mc_interface_try_input()) {
return;
}
@ -486,6 +510,10 @@ void mc_interface_set_pid_pos(float pos) {
}
void mc_interface_set_current(float current) {
if (fabsf(current) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -506,6 +534,10 @@ void mc_interface_set_current(float current) {
}
void mc_interface_set_brake_current(float current) {
if (fabsf(current) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -537,6 +569,10 @@ void mc_interface_set_brake_current(float current) {
* The relative current value, range [-1.0 1.0]
*/
void mc_interface_set_current_rel(float val) {
if (fabsf(val) > 0.001) {
SHUTDOWN_RESET();
}
mc_interface_set_current(val * m_conf.lo_current_motor_max_now);
}
@ -547,6 +583,10 @@ void mc_interface_set_current_rel(float val) {
* The relative current value, range [0.0 1.0]
*/
void mc_interface_set_brake_current_rel(float val) {
if (fabsf(val) > 0.001) {
SHUTDOWN_RESET();
}
mc_interface_set_brake_current(val * fabsf(m_conf.lo_current_motor_min_now));
}
@ -557,6 +597,10 @@ void mc_interface_set_brake_current_rel(float val) {
* The current value.
*/
void mc_interface_set_handbrake(float current) {
if (fabsf(current) > 0.001) {
SHUTDOWN_RESET();
}
if (mc_interface_try_input()) {
return;
}
@ -584,10 +628,16 @@ void mc_interface_set_handbrake(float current) {
* The relative current value, range [0.0 1.0]
*/
void mc_interface_set_handbrake_rel(float val) {
if (fabsf(val) > 0.001) {
SHUTDOWN_RESET();
}
mc_interface_set_handbrake(val * fabsf(m_conf.lo_current_motor_min_now));
}
void mc_interface_brake_now(void) {
SHUTDOWN_RESET();
mc_interface_set_duty(0.0);
}
@ -1013,6 +1063,32 @@ float mc_interface_read_reset_avg_iq(void) {
return DIR_MULT * res;
}
/**
* Read and reset the average direct axis motor voltage. (FOC only)
*
* @return
* The average D axis voltage.
*/
float mc_interface_read_reset_avg_vd(void) {
float res = m_motor_vd_sum / m_motor_vd_iterations;
m_motor_vd_sum = 0.0;
m_motor_vd_iterations = 0.0;
return DIR_MULT * res; // TODO: DIR_MULT?
}
/**
* Read and reset the average quadrature axis motor voltage. (FOC only)
*
* @return
* The average Q axis voltage.
*/
float mc_interface_read_reset_avg_vq(void) {
float res = m_motor_vq_sum / m_motor_vq_iterations;
m_motor_vq_sum = 0.0;
m_motor_vq_iterations = 0.0;
return DIR_MULT * res;
}
float mc_interface_get_pid_pos_set(void) {
return m_position_set;
}
@ -1172,6 +1248,45 @@ float mc_interface_get_distance_abs(void) {
return mc_interface_get_tachometer_abs_value(false) * tacho_scale;
}
setup_values mc_interface_get_setup_values(void) {
setup_values val = {0};
val.num_vescs = 1;
val.ah_tot += mc_interface_get_amp_hours(false);
val.ah_charge_tot += mc_interface_get_amp_hours_charged(false);
val.wh_tot += mc_interface_get_watt_hours(false);
val.wh_charge_tot += mc_interface_get_watt_hours_charged(false);
val.current_tot += mc_interface_get_tot_current_filtered();
val.current_in_tot += mc_interface_get_tot_current_in_filtered();
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
val.current_tot += msg->current;
val.num_vescs++;
}
can_status_msg_2 *msg2 = comm_can_get_status_msg_2_index(i);
if (msg2->id >= 0 && UTILS_AGE_S(msg2->rx_time) < 0.1) {
val.ah_tot += msg2->amp_hours;
val.ah_charge_tot += msg2->amp_hours_charged;
}
can_status_msg_3 *msg3 = comm_can_get_status_msg_3_index(i);
if (msg3->id >= 0 && UTILS_AGE_S(msg3->rx_time) < 0.1) {
val.wh_tot += msg3->watt_hours;
val.wh_charge_tot += msg3->watt_hours_charged;
}
can_status_msg_4 *msg4 = comm_can_get_status_msg_4_index(i);
if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 0.1) {
val.current_in_tot += msg4->current_in;
}
}
return val;
}
// MC implementation functions
/**
@ -1347,6 +1462,11 @@ void mc_interface_mc_timer_isr(void) {
m_motor_id_iterations++;
m_motor_iq_iterations++;
m_motor_vd_sum += mcpwm_foc_get_vd();
m_motor_vq_sum += mcpwm_foc_get_vq();
m_motor_vd_iterations++;
m_motor_vq_iterations++;
float abs_current = mc_interface_get_tot_current();
float abs_current_filtered = current;
if (m_conf.motor_type == MOTOR_TYPE_FOC) {
@ -1798,6 +1918,7 @@ static THD_FUNCTION(timer_thread, arg) {
// Relevant only in FOC mode with encoder enabled
if(m_conf.motor_type == MOTOR_TYPE_FOC &&
m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
mcpwm_foc_is_using_encoder() &&
encoder_spi_get_error_rate() > 0.05) {
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI);
}

3
mc_interface.h
View File

@ -67,6 +67,8 @@ float mc_interface_read_reset_avg_motor_current(void);
float mc_interface_read_reset_avg_input_current(void);
float mc_interface_read_reset_avg_id(void);
float mc_interface_read_reset_avg_iq(void);
float mc_interface_read_reset_avg_vd(void);
float mc_interface_read_reset_avg_vq(void);
float mc_interface_get_pid_pos_set(void);
float mc_interface_get_pid_pos_now(void);
float mc_interface_get_last_sample_adc_isr_duration(void);
@ -77,6 +79,7 @@ float mc_interface_get_battery_level(float *wh_left);
float mc_interface_get_speed(void);
float mc_interface_get_distance(void);
float mc_interface_get_distance_abs(void);
setup_values mc_interface_get_setup_values(void);
// MC implementation functions
void mc_interface_fault_stop(mc_fault_code fault);

18
mcpwm_foc.c
View File

@ -113,6 +113,7 @@ static volatile float m_last_adc_isr_duration;
static volatile float m_pos_pid_now;
static volatile bool m_init_done = false;
static volatile float m_gamma_now;
static volatile bool m_using_encoder;
#ifdef HW_HAS_3_SHUNTS
static volatile int m_curr2_sum;
@ -234,6 +235,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
m_last_adc_isr_duration = 0;
m_pos_pid_now = 0.0;
m_gamma_now = 0.0;
m_using_encoder = false;
memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
memset((void*)&m_samples, 0, sizeof(mc_sample_t));
@ -797,7 +799,6 @@ float mcpwm_foc_get_sampling_frequency_now(void) {
* Returns Ts used for virtual motor sync
*/
float mcpwm_foc_get_ts(void){
#ifdef HW_HAS_PHASE_SHUNTS
if (m_conf->foc_sample_v0_v7) {
return (1.0 / m_conf->foc_f_sw) ;
@ -807,8 +808,12 @@ float mcpwm_foc_get_ts(void){
#else
return (1.0 / m_conf->foc_f_sw) ;
#endif
}
bool mcpwm_foc_is_using_encoder(void) {
return m_using_encoder;
}
/**
* Calculate the current RPM of the motor. This is a signed value and the sign
* depends on the direction the motor is rotating in. Note that this value has
@ -2824,21 +2829,20 @@ static int read_hall(void) {
static float correct_encoder(float obs_angle, float enc_angle, float speed) {
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
static bool using_encoder = true;
// Hysteresis 5 % of total speed
float hyst = m_conf->foc_sl_erpm * 0.05;
if (using_encoder) {
if (m_using_encoder) {
if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
using_encoder = false;
m_using_encoder = false;
}
} else {
if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
using_encoder = true;
m_using_encoder = true;
}
}
return using_encoder ? enc_angle : obs_angle;
return m_using_encoder ? enc_angle : obs_angle;
}
static float correct_hall(float angle, float speed, float dt) {

2
mcpwm_foc.h
View File

@ -80,7 +80,7 @@ float mcpwm_foc_get_last_adc_isr_duration(void);
void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset);
void mcpwm_foc_set_current_offsets(volatile int curr0_offset, volatile int curr1_offset, volatile int curr2_offset);
float mcpwm_foc_get_ts(void);
void mcpwm_foc_reset_vd_vq(void);
bool mcpwm_foc_is_using_encoder(void);
// Interrupt handlers
void mcpwm_foc_tim_sample_int_handler(void);

10
nrf/nrf_driver.c
View File

@ -201,13 +201,21 @@ static THD_FUNCTION(tx_thread, arg) {
static uint8_t seq_cnt = 0;
seq_cnt++;
setup_values val = mc_interface_get_setup_values();
float wh_left = 0;
pl[index++] = MOTE_PACKET_ALIVE;
buffer_append_float16(pl, mc_interface_get_battery_level(0), 1e3, &index);
buffer_append_float16(pl, mc_interface_get_battery_level(&wh_left), 1e3, &index);
buffer_append_float32(pl, mc_interface_get_speed(), 1e3, &index);
buffer_append_float32(pl, mc_interface_get_distance_abs(), 1e3, &index);
buffer_append_float16(pl, mc_interface_temp_fet_filtered(), 1e1, &index);
buffer_append_float16(pl, mc_interface_temp_motor_filtered(), 1e1, &index);
pl[index++] = seq_cnt;
buffer_append_float32(pl, wh_left, 1e3, &index);
buffer_append_float32(pl, val.wh_tot, 1e4, &index);
buffer_append_float32(pl, val.wh_charge_tot, 1e4, &index);
pl[index++] = (uint8_t)((int8_t)(mc_interface_get_tot_current_directional_filtered() /
mc_interface_get_configuration()->lo_current_motor_max_now * 100.0));
if (driver_paused == 0) {
rf_tx_wrapper((char*)pl, index);

134
shutdown.c Normal file
View File

@ -0,0 +1,134 @@
/*
Copyright 2019 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/>.
*/
#include "shutdown.h"
#include "app.h"
#ifdef HW_SHUTDOWN_HOLD_ON
// Private variables
bool volatile m_button_pressed = false;
static volatile float m_inactivity_time = 0.0;
static THD_WORKING_AREA(shutdown_thread_wa, 128);
// Private functions
static THD_FUNCTION(shutdown_thread, arg);
void shutdown_init(void) {
chThdCreateStatic(shutdown_thread_wa, sizeof(shutdown_thread_wa), NORMALPRIO, shutdown_thread, NULL);
}
void shutdown_reset_timer(void) {
m_inactivity_time = 0.0;
}
bool shutdown_button_pressed(void) {
return m_button_pressed;
}
float shutdown_get_inactivity_time(void) {
return m_inactivity_time;
}
static THD_FUNCTION(shutdown_thread, arg) {
(void)arg;
chRegSetThreadName("Shutdown");
bool gates_disabled_here = false;
float gate_disable_time = 0.0;
systime_t last_iteration_time = chVTGetSystemTimeX();
for(;;) {
float dt = (float)chVTTimeElapsedSinceX(last_iteration_time) / (float)CH_CFG_ST_FREQUENCY;
last_iteration_time = chVTGetSystemTimeX();
const app_configuration *conf = app_get_configuration();
bool sample = HW_SAMPLE_SHUTDOWN();
bool clicked = m_button_pressed && !sample;
m_button_pressed = sample;
// Note: When the gates are enabled, the push to start function
// will prevent the regulator from shutting down. Therefore, the
// gate driver has to be disabled.
switch (conf->shutdown_mode) {
case SHUTDOWN_MODE_ALWAYS_OFF:
if (m_button_pressed) {
DISABLE_GATE();
gates_disabled_here = true;
HW_SHUTDOWN_HOLD_OFF();
}
break;
case SHUTDOWN_MODE_ALWAYS_ON:
HW_SHUTDOWN_HOLD_ON();
break;
default:
if (clicked) {
DISABLE_GATE();
gates_disabled_here = true;
HW_SHUTDOWN_HOLD_OFF();
}
break;
}
// If disabling the gates did not shut the VESC down within
// 2 seconds, enable the gates again.
if (gates_disabled_here && m_button_pressed) {
gate_disable_time += dt;
if (gate_disable_time > 2.0) {
ENABLE_GATE();
gates_disabled_here = false;
gate_disable_time = 0.0;
}
}
if (conf->shutdown_mode >= SHUTDOWN_MODE_OFF_AFTER_10S) {
m_inactivity_time += dt;
float shutdown_timeout = 0.0;
switch (conf->shutdown_mode) {
case SHUTDOWN_MODE_OFF_AFTER_10S: shutdown_timeout = 10.0; break;
case SHUTDOWN_MODE_OFF_AFTER_1M: shutdown_timeout = 60.0; break;
case SHUTDOWN_MODE_OFF_AFTER_5M: shutdown_timeout = 60.0 * 5.0; break;
case SHUTDOWN_MODE_OFF_AFTER_10M: shutdown_timeout = 60.0 * 10.0; break;
case SHUTDOWN_MODE_OFF_AFTER_30M: shutdown_timeout = 60.0 * 30.0; break;
case SHUTDOWN_MODE_OFF_AFTER_1H: shutdown_timeout = 60.0 * 60.0; break;
case SHUTDOWN_MODE_OFF_AFTER_5H: shutdown_timeout = 60.0 * 60.0 * 5.0; break;
default: break;
}
if (m_inactivity_time >= shutdown_timeout && m_button_pressed) {
DISABLE_GATE();
gates_disabled_here = true;
HW_SHUTDOWN_HOLD_OFF();
}
} else {
m_inactivity_time = 0.0;
}
chThdSleepMilliseconds(10);
}
}
#endif

33
conf_mc_app_default.h → shutdown.h
View File

@ -15,26 +15,27 @@
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 CONF_MC_APP_DEFAULT_H_
#define CONF_MC_APP_DEFAULT_H_
#ifndef SHUTDOWN_H_
#define SHUTDOWN_H_
#include "ch.h"
#include "hal.h"
#include "conf_general.h"
#include "hw.h"
// User defined default motor configuration file
#ifdef MCCONF_DEFAULT_USER
#include MCCONF_DEFAULT_USER
#ifdef HW_SHUTDOWN_HOLD_ON
#define SHUTDOWN_RESET() shutdown_reset_timer()
#define SHUTDOWN_BUTTON_PRESSED shutdown_button_pressed()
#else
#define SHUTDOWN_RESET()
#define SHUTDOWN_BUTTON_PRESSED false
#endif
// User defined default app configuration file
#ifdef APPCONF_DEFAULT_USER
#include APPCONF_DEFAULT_USER
#endif
// Fucntions
void shutdown_init(void);
void shutdown_reset_timer(void);
bool shutdown_button_pressed(void);
float shutdown_get_inactivity_time(void);
// Default configuration parameters that can be overridden
#include "mcconf_default.h"
#include "appconf_default.h"
#endif /* CONF_MC_APP_DEFAULT_H_ */
#endif /* SHUTDOWN_H_ */

23
terminal.c
View File

@ -109,7 +109,7 @@ void terminal_process_string(char *str) {
(double)(100.0 * (float)tp->p_time / (float)chVTGetSystemTimeX()));
tp = chRegNextThread(tp);
} while (tp != NULL);
commands_printf("");
commands_printf(" ");
} else if (strcmp(argv[0], "fault") == 0) {
commands_printf("%s\n", mc_interface_fault_to_string(mc_interface_get_fault()));
} else if (strcmp(argv[0], "faults") == 0) {
@ -799,6 +799,10 @@ void terminal_process_string(char *str) {
commands_printf(" Prints the status of the AS5047, AD2S1205, or Sin/Cos encoder.");
for (int i = 0;i < callback_write;i++) {
if (callbacks[i].cbf == 0) {
continue;
}
if (callbacks[i].arg_names) {
commands_printf("%s %s", callbacks[i].command, callbacks[i].arg_names);
} else {
@ -816,7 +820,7 @@ void terminal_process_string(char *str) {
} else {
bool found = false;
for (int i = 0;i < callback_write;i++) {
if (strcmp(argv[0], callbacks[i].command) == 0) {
if (callbacks[i].cbf != 0 && strcmp(argv[0], callbacks[i].command) == 0) {
callbacks[i].cbf(argc, (const char**)argv);
found = true;
break;
@ -874,6 +878,12 @@ void terminal_register_command_callback(
callback_num = i;
break;
}
// Check if the callback is empty (unregistered)
if (callbacks[i].cbf == 0) {
callback_num = i;
break;
}
}
callbacks[callback_num].command = command;
@ -888,3 +898,12 @@ void terminal_register_command_callback(
}
}
}
void terminal_unregister_callback(void(*cbf)(int argc, const char **argv)) {
for (int i = 0;i < callback_write;i++) {
if (callbacks[i].cbf == cbf) {
callbacks[i].cbf = 0;
}
}
}

1
terminal.h
View File

@ -30,5 +30,6 @@ void terminal_register_command_callback(
const char *help,
const char *arg_names,
void(*cbf)(int argc, const char **argv));
void terminal_unregister_callback(void(*cbf)(int argc, const char **argv));
#endif /* TERMINAL_H_ */