From 6481e00d0e49837140d9b89973ea1c88d0834a09 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 18:33:36 -0700 Subject: [PATCH 01/11] move out FOC voltage out of loop --- Firmware/MotorControl/low_level.c | 29 ++++++++++++++++------------- Firmware/MotorControl/low_level.h | 1 + 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3de25b22..d109aea0 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -877,19 +877,7 @@ __attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c * v_d - s * v_q; - float v_beta = c * v_q + s * v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; - } + FOC_voltage(motor, v_d, v_q); } } @@ -1155,6 +1143,21 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +void FOC_voltage(Motor_t* motor, float v_d, float v_q) { + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + if (!(check_timing(motor) < motor->control_deadline)) { + motor->error = ERROR_FOC_VOLTAGE_TIMING; + return; + } +} + bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { Current_control_t* ictrl = &motor->current_control; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 22a09877..6589e91e 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -237,6 +237,7 @@ void update_brake_current(); void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); +void FOC_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); From 35944a57504b5b377cadab9afe10a433ebc47ab1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 19:14:00 -0700 Subject: [PATCH 02/11] add motor types, including gimbal --- Firmware/MotorControl/low_level.c | 7 +++++-- Firmware/MotorControl/low_level.h | 7 +++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d109aea0..9f20227c 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -91,6 +91,8 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + // .motor_type = MOTOR_TYPE_GIMBAL, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { @@ -187,8 +189,9 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 6589e91e..697d0a84 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -60,6 +60,12 @@ typedef enum { CTRL_MODE_POSITION_CONTROL } Motor_control_mode_t; +typedef enum { + MOTOR_TYPE_HIGH_CURRENT, + // MOTOR_TYPE_LOW_CURRENT, //Not yet implemented + MOTOR_TYPE_GIMBAL +} Motor_type_t; + typedef struct { float phB; float phC; @@ -149,6 +155,7 @@ typedef struct { Iph_BC_t DC_calib; DRV8301_Obj gate_driver; DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers + Motor_type_t motor_type; float shunt_conductance; float phase_current_rev_gain; //Reverse gain for ADC to Amps Current_control_t current_control; From 2ef82d1edb62e60be3ce9a1044782947eb93bbda Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 19:50:26 -0700 Subject: [PATCH 03/11] implement gimbal motor mode --- Firmware/MotorControl/low_level.c | 40 +++++++++++++++++++++++-------- Firmware/MotorControl/low_level.h | 3 ++- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 9f20227c..232f0b03 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -48,6 +48,9 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public + +// NOTE: for gimbal motors, all units that are A are instead V. +// example: vel_gain is [V/(count/s)] instead. Motor_t motors[] = { { // M0 @@ -786,14 +789,20 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; - // #warning(hardcoded values for SK3-5065-280kv!) - // float R = 0.0332548246f; - // float L = 7.97315806e-06f; + float calibration_voltage = 0.0f; + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) + return false; + calibration_voltage = motor->calibration_current * motor->phase_resistance; - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) - return false; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + return false; + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + calibration_voltage = motor->calibration_current; + } else { return false; + } + if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) @@ -1146,7 +1155,7 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } -void FOC_voltage(Motor_t* motor, float v_d, float v_q) { +bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); @@ -1157,8 +1166,9 @@ void FOC_voltage(Motor_t* motor, float v_d, float v_q) { // Check we meet deadlines after queueing if (!(check_timing(motor) < motor->control_deadline)) { motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; + return false; } + return true; } bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { @@ -1314,8 +1324,18 @@ void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if (!FOC_current(motor, 0.0f, Iq)) { - break; // in case of error exit loop, motor->error has been set by FOC_current + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_current + } + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + //In gimbal motor mode, current is reinterptreted as voltage. + if(!FOC_voltage(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_voltage + } + } else { + motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + break; } update_brake_current(); diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 697d0a84..54fcb01e 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -49,6 +49,7 @@ typedef enum { ERROR_POS_CTRL_DURING_SENSORLESS, ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, } Error_t; // Note: these should be sorted from lowest level of control to @@ -244,7 +245,7 @@ void update_brake_current(); void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); -void FOC_voltage(Motor_t* motor, float v_d, float v_q); +bool FOC_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); From 6271a008d89d3f84e1dc891d95d60cc8da76e359 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 20:40:52 -0700 Subject: [PATCH 04/11] make gimbal note a bit easier to understand --- Firmware/MotorControl/low_level.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 232f0b03..4ff11ed1 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -49,8 +49,8 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public -// NOTE: for gimbal motors, all units that are A are instead V. -// example: vel_gain is [V/(count/s)] instead. +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] Motor_t motors[] = { { // M0 @@ -193,8 +193,8 @@ Motor_t motors[] = { }, // .gate_driver_regs Init by DRV8301_setup .motor_type = MOTOR_TYPE_HIGH_CURRENT, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. From f6b36345a2df70a3e84a763aed0652abe6096c87 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 16:53:50 -0800 Subject: [PATCH 05/11] fix voltage on enc calib --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 4ff11ed1..b44751fb 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -805,7 +805,7 @@ bool motor_calibration(Motor_t* motor) { if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) + if (!calib_enc_offset(motor, calibration_voltage)) return false; } From 44495601d5340ab2fe4beb7120d25cdba90696a0 Mon Sep 17 00:00:00 2001 From: Brandon Kinman Date: Mon, 22 Jan 2018 21:41:42 -0800 Subject: [PATCH 06/11] Update README.md Added short section about Gimbal mode selection. --- Firmware/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index a72a6eb6..8d663017 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,6 +70,15 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). + +### Motor Mode +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. + +If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. +If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. + +If 100's of mA current noise is "large" for you, and you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. ### Tuning parameters The most important parameters are the limits: From 8fb461dddd10b21096ef6fde7aea9cbc43545227 Mon Sep 17 00:00:00 2001 From: Brandon Kinman Date: Mon, 22 Jan 2018 21:57:50 -0800 Subject: [PATCH 07/11] Update README.md Made wording simpler for newbies. --- Firmware/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 8d663017..b1a29481 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -72,8 +72,10 @@ You must set: * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. * `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). -### Motor Mode -The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. +### Motor Modes +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. + +**Further detail:** If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. From cf1f60a62af1157b62fb3c7edd90e2f836fe3575 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:00:27 -0800 Subject: [PATCH 08/11] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index b1a29481..0d484e41 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,7 +70,7 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. -* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). ### Motor Modes The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. From 248a0882662fc70a02ddddaf088f23801d9f3133 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:00:43 -0800 Subject: [PATCH 09/11] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 0d484e41..07b88e66 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,7 +70,7 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. -* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). ### Motor Modes The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. From 7311cf1f42f761ea176975d3d82c3a4ebc9a8983 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:02:54 -0800 Subject: [PATCH 10/11] Update low_level.c --- Firmware/MotorControl/low_level.c | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index b44751fb..f945d2df 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -51,6 +51,7 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // NOTE: for gimbal motors, all units of A are instead V. // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. Motor_t motors[] = { { // M0 From 236b73a8b40aabc53abf3b508d2dce740babf095 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 23 Jan 2018 20:31:09 -0800 Subject: [PATCH 11/11] Update CHANGELOG.md --- Firmware/CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 566e8dc5..5150d704 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,8 @@ +## UNRELEASED + +### Added +* Gimbal motor mode + ## [0.3.1] - 2018-01-18 ### Added