Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Estimator] Move estimators for BLDC rotor into their own class #6

Merged
merged 1 commit into from
Oct 9, 2023
Merged

[Estimator] Move estimators for BLDC rotor into their own class #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions control_loop/bldc/brushless_6step_control_loop.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ class Brushless6StepControlLoopTest;
#include "bridge_3phase.hpp"
#include "control_loop.hpp"
#include "hal_clock.hpp"
#include "rotor_estimator.hpp"
namespace control_loop {

// Define a brushless 6step control loop class that inherits from ControlLoop
Expand Down Expand Up @@ -63,7 +64,7 @@ class Brushless6StepControlLoop : public ControlLoop {
void init(Brushless6StepControlLoopParams* params);

Brushless6StepControlLoop(hwbridge::Bridge3Phase& motor, basilisk_hal::HAL_CLOCK& clock,
hwbridge::BldcRotorSectorSensor* rotor_sensor = nullptr)
bldc_rotor_estimator::BldcRotorSectorSensor* rotor_sensor = nullptr)
: motor_(motor), clock_(clock) {
rotor_sensor_ = rotor_sensor;
}
Expand All @@ -74,7 +75,7 @@ class Brushless6StepControlLoop : public ControlLoop {
hwbridge::Bridge3Phase& motor_;
Brushless6StepControlLoopParams* params_ = nullptr;
basilisk_hal::HAL_CLOCK& clock_;
hwbridge::BldcRotorSectorSensor* rotor_sensor_;
bldc_rotor_estimator::BldcRotorSectorSensor* rotor_sensor_;
Brushless6StepControlLoopState state_ = Brushless6StepControlLoopState::STOP;
utime_t time_at_start_ = 0;
utime_t zero_crossing_time_ = 0;
Expand Down
113 changes: 0 additions & 113 deletions control_loop/bldc/brushless_foc_control_loop.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,117 +223,4 @@ BrushlessFocControlLoop::BrushlessFocControlLoopType BrushlessFocControlLoop::ge
return desired_control_loop_type;
}

app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::reset_estimation() {
app_hal_status_E ret = APP_HAL_OK;

rotor_position_ = 0.0f;
velocity_ = 0.0f;
compensated_velocity_ = 0.0f;
raw_hall_angle_ = 0.0f;
time_at_last_hall_update_ = 0;
time_update_last_called_ = 0;
number_of_hall_updates_ = 0;

// Update the rotor position with the sector sensor
uint8_t sector = 0;
ret = sector_sensor_.get_sector(sector);
if (ret == APP_HAL_OK) {
rotor_position_ = sector * 2.0 * M_PI / 6.0f;
}

return ret;
}

bool BldcElectricalRotorPositionEstimatorFromHall::is_estimation_valid() {
bool ret = false;
if (params_ != nullptr) {
bool num_hall_updates_to_start = (number_of_hall_updates_ > params_->num_hall_updates_to_start);
// Also make the return conditional if the position estimate no greater than the param for tolerance
bool rotor_position_tolerance = (fabs(rotor_position_ - raw_hall_angle_) < params_->max_estimate_angle_overrun);
ret = num_hall_updates_to_start && rotor_position_tolerance;
}
return ret;
}

app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::init(
BldcElectricalRotorPositionEstimatorFromHallParams_t* params) {
app_hal_status_E ret = APP_HAL_OK;

// Set the internal params pointer
params_ = params;

return ret;
}

app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::update(utime_t time) {
app_hal_status_E ret = APP_HAL_OK;
do {
if (params_ == nullptr) {
ret = APP_HAL_ERROR;
break;
}

uint8_t sector = 0;

ret = sector_sensor_.get_sector(sector);

if (ret != APP_HAL_OK) {
break;
}

float raw_hall_angle = sector * 2 * M_PI / 6;
// We should not update velocity estimations if the raw hall angle is the same as the previous one
if (fabs(raw_hall_angle - raw_hall_angle_) > 0.001f) {
// Calculate velocity with a simple differentiation
const float time_delta_since_hall_update =
(float)(time - time_at_last_hall_update_) / basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond;

float raw_hall_angle_diff = raw_hall_angle - raw_hall_angle_;

// Account for over/underflow
// NOTE: if the rotor delta theta is greater than pi radians, then this detection will not work
math::wraparound(raw_hall_angle_diff, static_cast<float>(-M_PI), static_cast<float>(M_PI));

velocity_ = raw_hall_angle_diff / time_delta_since_hall_update;

// Calculate a compensated velocity to account for position error and smoothly compensate for it
compensated_velocity_ = velocity_;
// TODO: implement compensated_velocity_ = velocity_ * (1 - ((rotor_position_ - raw_hall_angle_diff) /
// raw_hall_angle_diff));

rotor_position_ = raw_hall_angle;
this->raw_hall_angle_ = raw_hall_angle;
time_at_last_hall_update_ = time;
number_of_hall_updates_++;
}

const float current_measurement_period =
(float)(time - time_update_last_called_) / basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond;
// Update the rotor position with the velocity estimate
rotor_position_ += compensated_velocity_ * current_measurement_period;

// Implement a wraparound
math::wraparound(rotor_position_, 0.0f, float(2.0f * M_PI));

time_update_last_called_ = time;
} while (false);
return ret;
}

app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::get_rotor_position(float& rotor_position) {
app_hal_status_E ret = APP_HAL_OK;

rotor_position = rotor_position_;

return ret;
}

app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::get_rotor_velocity(float& rotor_velocity) {
app_hal_status_E ret = APP_HAL_OK;

rotor_velocity = compensated_velocity_;

return ret;
}

} // namespace control_loop
113 changes: 3 additions & 110 deletions control_loop/bldc/brushless_foc_control_loop.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,41 +13,10 @@ class BrushlessFocControlLoopTest;
#include "hal_clock.hpp"
#include "math_foc.hpp"
#include "pid.hpp"
#include "rotor_estimator.hpp"

namespace control_loop {

class BldcElectricalRotorPositionEstimator {
public:
BldcElectricalRotorPositionEstimator() = default;
virtual ~BldcElectricalRotorPositionEstimator() = default;

/**
* @brief Update the rotor position estimator
*/
virtual app_hal_status_E update(utime_t time) = 0;

/**
* @brief Get the rotor position
* @param rotor_position The rotor position as an electrical angle (radians)
*/
virtual app_hal_status_E get_rotor_position(float& rotor_position) = 0;

/**
* @brief Get the rotor velocity
* @param rotor_velocity The rotor velocity as an electrical angular velocity (radians/s)
*/
virtual app_hal_status_E get_rotor_velocity(float& rotor_velocity) = 0;

/**
* @brief get whether the rotor position estimation is valid
*
* @return true if the rotor position estimation is valid
*/
virtual bool is_estimation_valid() = 0;

virtual app_hal_status_E reset_estimation() = 0;
};

// Define a brushless foc control loop class that inherits from ControlLoop
class BrushlessFocControlLoop : public ControlLoop {
public:
Expand Down Expand Up @@ -89,7 +58,7 @@ class BrushlessFocControlLoop : public ControlLoop {
};

BrushlessFocControlLoop(hwbridge::Bridge3Phase& motor, basilisk_hal::HAL_CLOCK& clock,
BldcElectricalRotorPositionEstimator& rotor_position_estimator)
bldc_rotor_estimator::BldcElectricalRotorPositionEstimator& rotor_position_estimator)
: bridge_(motor), clock_(clock), rotor_position_estimator_(rotor_position_estimator) {}

/**
Expand All @@ -111,7 +80,7 @@ class BrushlessFocControlLoop : public ControlLoop {
BrushlessFocControlLoopState state_ = BrushlessFocControlLoopState::NOT_INITIALIZED;
hwbridge::Bridge3Phase& bridge_;
basilisk_hal::HAL_CLOCK& clock_;
BldcElectricalRotorPositionEstimator& rotor_position_estimator_;
bldc_rotor_estimator::BldcElectricalRotorPositionEstimator& rotor_position_estimator_;
// Control loop parameters
BrushlessFocControLoopParams* params_ = nullptr;

Expand Down Expand Up @@ -168,82 +137,6 @@ class BrushlessFocControlLoop : public ControlLoop {
#endif
};

class BldcElectricalRotorPositionEstimatorFromHall : public BldcElectricalRotorPositionEstimator {
public:
BldcElectricalRotorPositionEstimatorFromHall(basilisk_hal::HAL_CLOCK& clock, hwbridge::BldcRotorSectorSensor& sector_sensor)
: clock_(clock), sector_sensor_(sector_sensor) {}

typedef struct BldcElectricalRotorPositionEstimatorFromHallParams {
uint16_t num_hall_updates_to_start;
float max_estimate_angle_overrun; // How much to allow the estimator to overrun the hall angle (radians)
} BldcElectricalRotorPositionEstimatorFromHallParams_t;

/**
* @brief Initialize the rotor position estimator
* @param params The rotor position estimator parameters
* @return app_hal_status_E the status of the initialization
*/
app_hal_status_E init(BldcElectricalRotorPositionEstimatorFromHallParams_t* params);

/**
* @brief Update the rotor position estimator
* @param time The current time
* @return app_hal_status_E the status of the update
*/
app_hal_status_E update(utime_t time) override;

/**
* @brief Get the rotor position
* @param rotor_position The rotor position as an electrical angle (radians)
* @return app_hal_status_E the status of the operation
* @note: rotor_position is from 0 -> 2*pi
*/
app_hal_status_E get_rotor_position(float& rotor_position) override;

/**
* @brief Get the rotor velocity
* @param rotor_velocity The rotor velocity as an electrical angular velocity (radians/s)
* @return app_hal_status_E the status of the operation
*/
app_hal_status_E get_rotor_velocity(float& rotor_velocity) override;

/**
* @brief Get the raw hall angle
* @param raw_hall_angle The raw hall angle (radians)
* @return app_hal_status_E the status of the operation
* @note: raw_hall_angle is from 0 -> 2*pi
*/
app_hal_status_E get_raw_hall_angle(float& raw_hall_angle) {
raw_hall_angle = raw_hall_angle_;
return APP_HAL_OK;
}

/**
* @brief get whether the rotor position estimation is valid
* @return true if the rotor position estimation is valid
* @note: the rotor position estimation is valid if the number of hall updates is greater than the number of hall updates to
* start
*/
bool is_estimation_valid() override;

/**
* @brief reset the rotor position estimation
* @return app_hal_status_E the status of the operation
*/
app_hal_status_E reset_estimation() override;

basilisk_hal::HAL_CLOCK& clock_;
hwbridge::BldcRotorSectorSensor& sector_sensor_;
float rotor_position_ = 0.0f;
float velocity_ = 0.0f; // rad/s
float compensated_velocity_ = 0.0f; // rad/s
float raw_hall_angle_ = 0.0f;
utime_t time_at_last_hall_update_ = 0;
utime_t time_update_last_called_ = 0;
uint64_t number_of_hall_updates_ = 0;
BldcElectricalRotorPositionEstimatorFromHallParams_t* params_ = nullptr;
};

} // namespace control_loop

#endif // BRUSHLESS_FOC_CONTROL_LOOP_HPP
9 changes: 0 additions & 9 deletions control_loop/bldc/math_foc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,8 @@

#include <stdint.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace math {

constexpr float sqrt_3 = 1.7320508075688772;
constexpr float sqrt_3_over_3 = 0.5773502691896258;

constexpr float M_PI_FLOAT = static_cast<float>(M_PI);

typedef struct {
float alpha;
float beta;
Expand Down
Loading
Loading