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

feat(flex-stacker): stream motor driver stall guard value during move #468

Open
wants to merge 10 commits into
base: edge
Choose a base branch
from
Open

feat(flex-stacker): stream motor driver stall guard value during move #468

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3499fac
create interface before driver starts running
ahiuchingau Oct 3, 2024
03ed182
send poll and stop poll message from motor task
ahiuchingau Oct 2, 2024
922d061
add gcode and messages
ahiuchingau Oct 3, 2024
94c1dbd
can report 1 all the time during move
ahiuchingau Oct 3, 2024
5a9b059
new task now works
ahiuchingau Oct 3, 2024
ffa9f8e
thank you universe
ahiuchingau Oct 4, 2024
6ac3e65
add some comments
ahiuchingau Oct 4, 2024
e2a3ab2
add stream stallguard as an optional param for move messages
ahiuchingau Oct 4, 2024
144d9bd
fix lint
ahiuchingau Oct 4, 2024
5b80425
format
ahiuchingau Oct 4, 2024
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
55 changes: 54 additions & 1 deletion stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_driver_task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,10 @@
#include "FreeRTOS.h"
#include "climits"
#include "firmware/freertos_tasks.hpp"
#include "firmware/motor_driver_policy.hpp"
#include "firmware/motor_hardware.h"
#include "flex-stacker/motor_driver_task.hpp"
#include "flex-stacker/tmc2160_interface.hpp"
#include "ot_utils/freertos/freertos_timer.hpp"

namespace motor_driver_task {
Expand All @@ -20,6 +22,48 @@ static tasks::FirmwareTasks::MotorDriverQueue
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static auto _top_task = motor_driver_task::MotorDriverTask(_queue, nullptr);

static constexpr uint32_t STREAM_TASK_DEPTH = 200;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static StaticTask_t stream_task_buffer;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static std::array<StackType_t, STREAM_TASK_DEPTH> stream_task_stack;
static constexpr uint32_t FREQ_MS = 10;

static void run_stallguard_task(void* arg) {
auto* interface = static_cast<
tmc2160::TMC2160Interface<motor_driver_policy::MotorDriverPolicy>*>(
arg);

uint32_t ulNotifiedValue = 0;
MotorID motor_id = MotorID::MOTOR_X;

while (true) {
// wait for a new notification
xTaskNotifyWait(0, ULONG_MAX, &ulNotifiedValue, portMAX_DELAY);
if (ulNotifiedValue > 0) {
motor_id = (ulNotifiedValue == 1) ? MotorID::MOTOR_X
: (ulNotifiedValue == 2) ? MotorID::MOTOR_Z
: MotorID::MOTOR_L;
static_cast<void>(interface->read_stallguard(motor_id));
for (;;) {
if (xTaskNotifyWait(ULONG_MAX, ULONG_MAX, nullptr, 0) ==
pdPASS) {
// Received a notification! This notification should only be
// used to break the for loop and the value would not be
// read. This, together with the task suspend issued by the
// top task, makes sure that we can't just switch from the
// current motor to another one immediately
break;
}
auto result = interface->read_stallguard(motor_id);
auto msg = messages::StallGuardResultMessage{.data = result};
static_cast<void>(_queue.try_send_from_isr(msg));
vTaskDelay(pdMS_TO_TICKS(FREQ_MS));
}
}
}
}

auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
auto* handle = xTaskGetCurrentTaskHandle();
_queue.provide_handle(handle);
Expand All @@ -29,8 +73,17 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
spi_hardware_init();

auto policy = motor_driver_policy::MotorDriverPolicy();
auto tmc2160_interface = tmc2160::TMC2160Interface(policy);

auto* stream_handle = xTaskCreateStatic(
run_stallguard_task, "Stallguard Task", STREAM_TASK_DEPTH,
&tmc2160_interface, 1, stream_task_stack.data(), &stream_task_buffer);
vTaskSuspend(stream_handle);

_top_task.provide_stallguard_handle(stream_handle);

while (true) {
_top_task.run_once(policy);
_top_task.run_once(tmc2160_interface);
}
}

Expand Down
2 changes: 1 addition & 1 deletion stm32-modules/flex-stacker/firmware/motor_control/motor_spi_hardware.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -279,7 +279,7 @@ void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
bool motor_spi_sendreceive(
MotorID motor_id, uint8_t *txData, uint8_t *rxData, uint16_t size
) {
const TickType_t max_block_time = pdMS_TO_TICKS(100);
const TickType_t max_block_time = pdMS_TO_TICKS(10);
uint32_t notification_val = 0;

if (!_spi.initialized || (_spi.task_to_notify != NULL) || (size > MOTOR_MAX_SPI_LEN)) {
Expand Down
2 changes: 1 addition & 1 deletion stm32-modules/include/flex-stacker/firmware/firmware_tasks.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ constexpr uint8_t SYSTEM_TASK_PRIORITY = 1;
constexpr size_t MOTOR_STACK_SIZE = 256;
constexpr uint8_t MOTOR_TASK_PRIORITY = 1;

constexpr size_t MOTOR_DRIVER_STACK_SIZE = 256;
constexpr size_t MOTOR_DRIVER_STACK_SIZE = 512;
constexpr uint8_t MOTOR_DRIVER_TASK_PRIORITY = 1;

constexpr size_t UI_STACK_SIZE = 256;
Expand Down
75 changes: 69 additions & 6 deletions stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,37 @@ struct ArgNoPrefix {
ValueType value = ValueType{};
};

struct StallGuardResult {
uint32_t data;

using ParseResult = std::optional<StallGuardResult>;
static constexpr auto prefix = std::array{'M', '9', '0', '0', ' '};

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
auto working = prefix_matches(input, limit, prefix);
if (working == input) {
return std::make_pair(ParseResult(), input);
}
return std::make_pair(ParseResult(StallGuardResult()), working);
}

template <typename InputIt, typename InLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputIt, InLimit>
static auto write_response_into(InputIt buf, InLimit limit, uint32_t data)
-> InputIt {
auto res = snprintf(&*buf, (limit - buf), "M900 %lu OK\n", data);
if (res <= 0) {
return buf;
}
return buf + res;
}
};

struct GetTMCRegister {
MotorID motor_id;
uint8_t reg;
Expand Down Expand Up @@ -409,6 +440,7 @@ struct MoveMotorInSteps {
int32_t steps;
uint32_t steps_per_second;
uint32_t steps_per_second_sq;
bool stream_stallguard;

using ParseResult = std::optional<MoveMotorInSteps>;
static constexpr auto prefix = std::array{'G', '0', '.', 'S', ' '};
Expand All @@ -419,15 +451,16 @@ struct MoveMotorInSteps {
using LArg = Arg<int32_t, 'L'>;
using FreqArg = Arg<uint32_t, 'F'>;
using RampArg = Arg<uint32_t, 'R'>;
using StreamArg = ArgNoVal<'S'>;

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, FreqArg,
RampArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, FreqArg, RampArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
Expand All @@ -436,6 +469,7 @@ struct MoveMotorInSteps {
.steps = 0,
.steps_per_second = 0,
.steps_per_second_sq = 0,
.stream_stallguard = false,
};

auto arguments = res.first.value();
Expand All @@ -460,6 +494,12 @@ struct MoveMotorInSteps {
if (std::get<4>(arguments).present) {
ret.steps_per_second_sq = std::get<4>(arguments).value;
}

// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
if (std::get<5>(arguments).present) {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -474,6 +514,7 @@ struct MoveMotorInSteps {
struct MoveMotorInMm {
MotorID motor_id;
float mm;
bool stream_stallguard;
std::optional<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveMotorInMm>;
Expand All @@ -486,21 +527,23 @@ struct MoveMotorInMm {
using VelArg = Arg<float, 'V'>;
using AccelArg = Arg<float, 'A'>;
using DiscontArg = Arg<float, 'D'>;
using StreamArg = ArgNoVal<'S'>;

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg,
DiscontArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg, DiscontArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
auto ret = MoveMotorInMm{
.motor_id = MotorID::MOTOR_X,
.mm = 0.0,
.stream_stallguard = false,
.mm_per_second = std::nullopt,
.mm_per_second_sq = std::nullopt,
.mm_per_second_discont = std::nullopt,
Expand Down Expand Up @@ -531,6 +574,11 @@ struct MoveMotorInMm {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.mm_per_second_discont = std::get<5>(arguments).value;
}
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
if (std::get<6>(arguments).present) {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -545,6 +593,7 @@ struct MoveMotorInMm {
struct MoveToLimitSwitch {
MotorID motor_id;
bool direction;
bool stream_stallguard;
std::optional<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveToLimitSwitch>;
Expand All @@ -557,21 +606,23 @@ struct MoveToLimitSwitch {
using VelArg = Arg<float, 'V'>;
using AccelArg = Arg<float, 'A'>;
using DiscontArg = Arg<float, 'D'>;
using StreamArg = ArgNoVal<'S'>;

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg,
DiscontArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg, DiscontArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
auto ret = MoveToLimitSwitch{
.motor_id = MotorID::MOTOR_X,
.direction = false,
.stream_stallguard = false,
.mm_per_second = std::nullopt,
.mm_per_second_sq = std::nullopt,
.mm_per_second_discont = std::nullopt,
Expand Down Expand Up @@ -604,6 +655,12 @@ struct MoveToLimitSwitch {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.mm_per_second_discont = std::get<5>(arguments).value;
}

// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
if (std::get<6>(arguments).present) {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -619,6 +676,7 @@ struct MoveMotor {
MotorID motor_id;
bool direction;
uint32_t frequency;
bool stream_stallguard;

using ParseResult = std::optional<MoveMotor>;
static constexpr auto prefix = std::array{'G', '5', ' '};
Expand All @@ -628,6 +686,7 @@ struct MoveMotor {
using ZArg = Arg<int, 'Z'>;
using LArg = Arg<int, 'L'>;
using FreqArg = Arg<uint32_t, 'F'>;
using StreamArg = ArgNoVal<'S'>;

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
Expand All @@ -643,6 +702,7 @@ struct MoveMotor {
.motor_id = MotorID::MOTOR_X,
.direction = false,
.frequency = 0,
.stream_stallguard = false,
};

auto arguments = res.first.value();
Expand All @@ -661,6 +721,9 @@ struct MoveMotor {
if (std::get<3>(arguments).present) {
ret.frequency = std::get<3>(arguments).value;
}
if (std::get<4>(arguments).present) {
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand Down
24 changes: 19 additions & 5 deletions stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,6 +246,15 @@ class HostCommsTask {
cache_entry);
}

template <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
auto visit_message(const messages::StallGuardResultMessage& response,
InputIt tx_into, InputLimit tx_limit) -> InputIt {
return gcode::StallGuardResult::write_response_into(tx_into, tx_limit,
response.data);
}

template <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
Expand Down Expand Up @@ -536,7 +545,8 @@ class HostCommsTask {
.motor_id = gcode.motor_id,
.steps = gcode.steps,
.steps_per_second = gcode.steps_per_second,
.steps_per_second_sq = gcode.steps_per_second_sq};
.steps_per_second_sq = gcode.steps_per_second_sq,
.stream_stallguard = gcode.stream_stallguard};
if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
auto wrote_to = errors::write_into(
tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
Expand All @@ -561,6 +571,7 @@ class HostCommsTask {
.id = id,
.motor_id = gcode.motor_id,
.mm = gcode.mm,
.stream_stallguard = gcode.stream_stallguard,
.mm_per_second = gcode.mm_per_second,
.mm_per_second_sq = gcode.mm_per_second_sq,
.mm_per_second_discont = gcode.mm_per_second_discont};
Expand Down Expand Up @@ -588,6 +599,7 @@ class HostCommsTask {
.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.stream_stallguard = gcode.stream_stallguard,
.mm_per_second = gcode.mm_per_second,
.mm_per_second_sq = gcode.mm_per_second_sq,
.mm_per_second_discont = gcode.mm_per_second_discont};
Expand All @@ -611,10 +623,12 @@ class HostCommsTask {
false, errors::write_into(tx_into, tx_limit,
errors::ErrorCode::GCODE_CACHE_FULL));
}
auto message = messages::MoveMotorMessage{.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.frequency = gcode.frequency};
auto message = messages::MoveMotorMessage{
.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.frequency = gcode.frequency,
.stream_stallguard = gcode.stream_stallguard};
if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
auto wrote_to = errors::write_into(
tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
Expand Down
Loading
Loading