mirror of https://github.com/rusefi/bldc.git
Merge branch 'master' of https://github.com/vedderb/bldc
This commit is contained in:
commit
ce31092c1d
|
@ -283,17 +283,11 @@
|
|||
#ifndef APPCONF_BALANCE_HERTZ
|
||||
#define APPCONF_BALANCE_HERTZ 1000
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_M_AXIS
|
||||
#define APPCONF_BALANCE_M_AXIS 0
|
||||
#ifndef APPCONF_BALANCE_PITCH_FAULT
|
||||
#define APPCONF_BALANCE_PITCH_FAULT 20
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_C_AXIS
|
||||
#define APPCONF_BALANCE_C_AXIS 1
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_M_FAULT
|
||||
#define APPCONF_BALANCE_M_FAULT 20
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_C_FAULT
|
||||
#define APPCONF_BALANCE_C_FAULT 45
|
||||
#ifndef APPCONF_BALANCE_ROLL_FAULT
|
||||
#define APPCONF_BALANCE_ROLL_FAULT 45
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_USE_SWITCHES
|
||||
#define APPCONF_BALANCE_USE_SWITCHES false
|
||||
|
@ -316,11 +310,11 @@
|
|||
#ifndef APPCONF_BALANCE_TILTBACK_LOW_V
|
||||
#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_M_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_M_TOLERANCE 20.0
|
||||
#ifndef APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE 20.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_C_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_C_TOLERANCE 8.0
|
||||
#ifndef APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE 8.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_SPEED
|
||||
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
|
||||
|
|
|
@ -103,6 +103,10 @@ void app_set_configuration(app_configuration *conf) {
|
|||
|
||||
case APP_BALANCE:
|
||||
app_balance_start();
|
||||
if(appconf.imu_conf.type == IMU_TYPE_INTERNAL){
|
||||
hw_stop_i2c();
|
||||
app_uartcomm_start();
|
||||
}
|
||||
break;
|
||||
|
||||
case APP_NRF:
|
||||
|
|
|
@ -58,8 +58,8 @@ void app_balance_start(void);
|
|||
void app_balance_stop(void);
|
||||
void app_balance_configure(balance_config *conf, imu_config *conf2);
|
||||
float app_balance_get_pid_output(void);
|
||||
float app_balance_get_m_angle(void);
|
||||
float app_balance_get_c_angle(void);
|
||||
float app_balance_get_pitch_angle(void);
|
||||
float app_balance_get_roll_angle(void);
|
||||
uint32_t app_balance_get_diff_time(void);
|
||||
float app_balance_get_motor_current(void);
|
||||
float app_balance_get_motor_position(void);
|
||||
|
|
|
@ -56,7 +56,7 @@ static thread_t *app_thread;
|
|||
|
||||
// Values used in loop
|
||||
static BalanceState state;
|
||||
static float m_angle, c_angle;
|
||||
static float pitch_angle, roll_angle;
|
||||
static float proportional, integral, derivative;
|
||||
static float last_proportional;
|
||||
static float pid_value;
|
||||
|
@ -80,8 +80,8 @@ void app_balance_start(void) {
|
|||
|
||||
// Reset all Values
|
||||
state = STARTUP;
|
||||
m_angle = 0;
|
||||
c_angle = 0;
|
||||
pitch_angle = 0;
|
||||
roll_angle = 0;
|
||||
switches_value = 0;
|
||||
proportional = 0;
|
||||
integral = 0;
|
||||
|
@ -122,11 +122,11 @@ void app_balance_stop(void) {
|
|||
float app_balance_get_pid_output(void) {
|
||||
return pid_value;
|
||||
}
|
||||
float app_balance_get_m_angle(void) {
|
||||
return m_angle;
|
||||
float app_balance_get_pitch_angle(void) {
|
||||
return pitch_angle;
|
||||
}
|
||||
float app_balance_get_c_angle(void) {
|
||||
return c_angle;
|
||||
float app_balance_get_roll_angle(void) {
|
||||
return roll_angle;
|
||||
}
|
||||
uint32_t app_balance_get_diff_time(void) {
|
||||
return ST2US(diff_time);
|
||||
|
@ -193,28 +193,8 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
motor_position = mc_interface_get_pid_pos_now();
|
||||
|
||||
// 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;
|
||||
}
|
||||
pitch_angle = imu_get_pitch() * 180.0f / M_PI;
|
||||
roll_angle = imu_get_roll() * 180.0f / M_PI;
|
||||
|
||||
if(!balance_conf.use_switches){
|
||||
switches_value = 2;
|
||||
|
@ -248,8 +228,8 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
|
||||
// Check for fault
|
||||
if(
|
||||
fabsf(m_angle) > balance_conf.m_fault || // Balnce axis tip over
|
||||
fabsf(c_angle) > balance_conf.c_fault || // Cross axis tip over
|
||||
fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over
|
||||
fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over
|
||||
app_balance_get_switch_value() == 0 || // Switch fully open
|
||||
(app_balance_get_switch_value() == 1 && fabsf(mc_interface_get_duty_cycle_now()) < 0.003) // Switch partially open and stopped
|
||||
){
|
||||
|
@ -283,7 +263,7 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
}
|
||||
|
||||
// Do PID maths
|
||||
proportional = setpoint - m_angle;
|
||||
proportional = setpoint - pitch_angle;
|
||||
// Apply deadzone
|
||||
proportional = apply_deadzone(proportional);
|
||||
// Resume real PID maths
|
||||
|
@ -313,8 +293,8 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
break;
|
||||
case (FAULT):
|
||||
// Check for valid startup position and switch state
|
||||
if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance && app_balance_get_switch_value() == 2){
|
||||
setpoint = m_angle;
|
||||
if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && app_balance_get_switch_value() == 2){
|
||||
setpoint = pitch_angle;
|
||||
setpoint_target = 0;
|
||||
setpointAdjustmentType = CENTERING;
|
||||
state = RUNNING;
|
||||
|
|
|
@ -515,8 +515,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
|
|||
uint8_t send_buffer[50];
|
||||
send_buffer[ind++] = COMM_GET_DECODED_BALANCE;
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_pid_output() * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_m_angle() * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_c_angle() * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_pitch_angle() * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll_angle() * 1000000.0), &ind);
|
||||
buffer_append_uint32(send_buffer, app_balance_get_diff_time(), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_position() * 1000000.0), &ind);
|
||||
|
|
|
@ -227,10 +227,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
|
|||
buffer_append_float32_auto(buffer, conf->app_balance_conf.ki, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.kd, &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.pitch_fault, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind);
|
||||
buffer[ind++] = conf->app_balance_conf.use_switches;
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
|
||||
|
@ -238,8 +236,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
|
|||
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_speed, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_high_voltage, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_low_voltage, &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_pitch_tolerance, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_roll_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);
|
||||
|
@ -496,10 +494,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
|
|||
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.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.pitch_fault = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.use_switches = 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);
|
||||
|
@ -507,8 +503,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
|
|||
conf->app_balance_conf.tiltback_speed = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.tiltback_high_voltage = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.tiltback_low_voltage = 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_pitch_tolerance = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.startup_roll_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);
|
||||
|
@ -749,10 +745,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
|
|||
conf->app_balance_conf.ki = APPCONF_BALANCE_KI;
|
||||
conf->app_balance_conf.kd = APPCONF_BALANCE_KD;
|
||||
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.pitch_fault = APPCONF_BALANCE_PITCH_FAULT;
|
||||
conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT;
|
||||
conf->app_balance_conf.use_switches = APPCONF_BALANCE_USE_SWITCHES;
|
||||
conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY;
|
||||
conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
|
||||
|
@ -760,8 +754,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
|
|||
conf->app_balance_conf.tiltback_speed = APPCONF_BALANCE_TILTBACK_SPEED;
|
||||
conf->app_balance_conf.tiltback_high_voltage = APPCONF_BALANCE_TILTBACK_HIGH_V;
|
||||
conf->app_balance_conf.tiltback_low_voltage = APPCONF_BALANCE_TILTBACK_LOW_V;
|
||||
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_pitch_tolerance = APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE;
|
||||
conf->app_balance_conf.startup_roll_tolerance = APPCONF_BALANCE_STARTUP_ROLL_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;
|
||||
|
|
|
@ -9,7 +9,7 @@
|
|||
|
||||
// Constants
|
||||
#define MCCONF_SIGNATURE 503309878
|
||||
#define APPCONF_SIGNATURE 3900592330
|
||||
#define APPCONF_SIGNATURE 783041200
|
||||
|
||||
// Functions
|
||||
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
||||
|
|
17
datatypes.h
17
datatypes.h
|
@ -489,22 +489,13 @@ typedef struct {
|
|||
bool send_crc_ack;
|
||||
} nrf_config;
|
||||
|
||||
// External LED state
|
||||
typedef enum {
|
||||
PITCH = 0,
|
||||
ROLL,
|
||||
YAW
|
||||
} AXIS;
|
||||
|
||||
typedef struct {
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
uint16_t hertz;
|
||||
AXIS m_axis;
|
||||
AXIS c_axis;
|
||||
float m_fault;
|
||||
float c_fault;
|
||||
float pitch_fault;
|
||||
float roll_fault;
|
||||
bool use_switches;
|
||||
float overspeed_duty;
|
||||
float tiltback_duty;
|
||||
|
@ -512,8 +503,8 @@ typedef struct {
|
|||
float tiltback_speed;
|
||||
float tiltback_high_voltage;
|
||||
float tiltback_low_voltage;
|
||||
float startup_m_tolerance;
|
||||
float startup_c_tolerance;
|
||||
float startup_pitch_tolerance;
|
||||
float startup_roll_tolerance;
|
||||
float startup_speed;
|
||||
float deadzone;
|
||||
float current_boost;
|
||||
|
|
Loading…
Reference in New Issue