novirium
/
airseeder-firmware
1
0
Fork 0
Code Issues 4 Pull Requests Projects Releases Wiki Activity

Fix bug reporting speed as halved, reorganise roller speed

Browse Source
master
Tom Wilson 4 years ago
parent 726550d1e6
commit 4f7f4c18bc
2 changed files with 55 additions and 49 deletions
Show Stats Download Patch File Download Diff File

100
src/motor-esp32/roller_speed.cpp
Unescape View File

@ -8,77 +8,82 @@
#include "freertos/queue.h"
#include "soc/rtc.h"
static uint32_t roller_pulses[3] = {0, 0, 0}; // Total pulses of roller so far
static uint32_t roller_last_neg_edge_time[3] = {0, 0, 0}; // Precision timer value of most recent falling edge
static uint32_t roller_last_pos_edge_time[3] = {0, 0, 0}; // Precision timer value of most recent rising edge
static bool roller_pos_edge_seen[3] = {false, false,
false}; // Track if if first pulse edge has been seen yet to compare to
static bool roller_neg_edge_seen[3] = {false, false, false};
// Also store timestamp as microseconds since start to avoid overflow of the precision timer when checking for stop
static int64_t roller_last_timestamp[3] = {0, 0, 0};
// We track both edges of the pulse signal in parallel to increase resolution
typedef enum { EDGE_POS = 0, EDGE_NEG = 1 } edge_type_t;
// Use a single item queue for each measurement we need to distribute.
// This avoids needing to create a locked buffer system of our own.
static xQueueHandle roller_measurement_queue[3];
// Struct to hold all the current data for tracking the pulse edges - only used within the pulse interrupt
typedef struct {
uint32_t edge_count = 0; // Running count of this type of edge
int64_t last_edge_precision_time = 0; // Precision timer value of most recent edge
uint32_t last_edge_timestamp = 0; // Also store timestamp as microseconds since start to avoid overflow of the
// precision timer when checking for stop
bool edge_seen = false; // Track if if first pulse edge has been seen yet to compare to
} edge_record_t;
// Store current edge data for interrupt for each edge type, for each roller.
static edge_record_t edge_records[3][2];
// struct to hold move our pulse measurements from our ISR via the queue
typedef struct {
uint32_t period; // period since last pulse in APB clock ticks
int64_t timestamp; // time measurement occurred in microseconds since boot
uint32_t pulses; // Total pulses of roller so far
uint32_t period; // period since last pulse in APB clock ticks
int64_t timestamp; // time measurement occurred in microseconds since boot
uint32_t total_edges; // Total number of pulse edges so far (both rising and falling)
} pulse_measurement_t;
// Use a single item queue for each measurement we need to distribute.
// This avoids needing to create a locked buffer system of our own.
static xQueueHandle roller_measurement_queue[3];
// Gets called by the MCPWM capture module. Tells us which capture unit it was, which edge, and when it happened
static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig,
const cap_event_data_t *edata, void *arg) {
// Not strictly necessary to do the wakeup check here, as we shouldn't be blokcing anything. Doesn't hurt though.
// Not strictly necessary to do the wakeup check here, as we shouldn't be blocking anything. Doesn't hurt though.
BaseType_t high_task_wakeup = pdFALSE;
int r_id = 0;
int64_t time_now = esp_timer_get_time();
roller_id_t r_id;
if (cap_sig == MCPWM_SELECT_CAP0) {
r_id = 0;
r_id = ROLLER_1;
} else if (cap_sig == MCPWM_SELECT_CAP1) {
r_id = 1;
r_id = ROLLER_2;
} else if (cap_sig == MCPWM_SELECT_CAP2) {
r_id = 2;
r_id = ROLLER_3;
} else {
return false;
}
// Add one to our revs counter
roller_pulses[r_id]++;
// Create a pointer to our relevant edge record
edge_record_t *edge_record;
if (edata->cap_edge == MCPWM_POS_EDGE) {
edge_record = &edge_records[r_id][EDGE_POS];
} else {
edge_record = &edge_records[r_id][EDGE_NEG];
}
pulse_measurement_t new_measurement;
int64_t time_now;
// Add one to our edge counter
edge_record->edge_count++;
time_now = esp_timer_get_time();
// Make a new measurement to hand our via the queue. Even if there is no measured period (and therefor no speed
// update) we still want to update the edge count and timestamp
pulse_measurement_t new_measurement;
new_measurement.timestamp = time_now;
new_measurement.pulses = roller_pulses[r_id];
new_measurement.period = 0; // default to 0 - no speed - if we don't actually have a period to put in but still want
// to give an update with the new pulse count
// Just make sure we have a previous edge to compare to, and that it was no more than ZERO_SPEED_TIMEOUT_US ago
if ((roller_pos_edge_seen[r_id] or roller_neg_edge_seen[r_id]) and ((time_now - roller_last_timestamp[r_id]) < ZERO_SPEED_TIMEOUT_US)) {
if ((edata->cap_edge == MCPWM_POS_EDGE) and (roller_pos_edge_seen[r_id])) {
new_measurement.period = edata->cap_value - roller_last_pos_edge_time[r_id];
} else if ((edata->cap_edge == MCPWM_NEG_EDGE) and (roller_neg_edge_seen[r_id])) {
new_measurement.period = edata->cap_value - roller_last_neg_edge_time[r_id];
}
}
new_measurement.total_edges = edge_records[r_id][EDGE_POS].edge_count + edge_records[r_id][EDGE_NEG].edge_count;
new_measurement.period = 0;
// Only calculate a period if we have a previous edge to compare to, and that it was no more than
// ZERO_SPEED_TIMEOUT_US ago
if ((edge_record->edge_seen) and ((time_now - edge_record->last_edge_timestamp) < ZERO_SPEED_TIMEOUT_US)) {
new_measurement.period = edata->cap_value - edge_record->last_edge_precision_time;
}
// Send our new measurement out into the queue to be read by other tasks when required
xQueueOverwriteFromISR(roller_measurement_queue[r_id], &new_measurement, &high_task_wakeup);
if (edata->cap_edge == MCPWM_POS_EDGE){
roller_last_pos_edge_time[r_id] = edata->cap_value;
roller_pos_edge_seen[r_id] = true;
} else {
roller_last_neg_edge_time[r_id] = edata->cap_value;
roller_neg_edge_seen[r_id] = true;
}
roller_last_timestamp[r_id] = time_now;
// Record the edge data for future comparison in this interrupt
edge_record->edge_seen = true;
edge_record->last_edge_precision_time = edata->cap_value;
edge_record->last_edge_timestamp = time_now;
return high_task_wakeup == pdTRUE;
}
@ -100,7 +105,8 @@ float get_roller_speed(roller_id_t roller_id) {
float get_roller_revs(roller_id_t roller_id) {
pulse_measurement_t pulse_measurement;
xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0);
return (float)pulse_measurement.pulses / (float)ROLLER_PULSES_PER_REV;
// Two edges per pulse
return (float)pulse_measurement.total_edges / ((float)ROLLER_PULSES_PER_REV * 2.0);
}
void roller_speed_setup(void) {

4
src/motor-esp32/roller_speed.h
Unescape View File

@ -4,8 +4,8 @@
#include "../common/shared_structs.h"
// Number of sensor pulses per roller rev. Note that this will overflow at about 4 billion pulses.
// Roller physically has 30 pulses per rev, but we're counting both rising and falling edges
#define ROLLER_PULSES_PER_REV 60
// Roller physically has 30 pulses per rev
#define ROLLER_PULSES_PER_REV 30
// Time without a pulse after which the speed will be considered to be 0.
// Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds

Write Preview
Loading…
Cancel
Save