Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-21 18:56:31 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fixed reversed move coming out of hold-flush. Look in planner.cpp / planner_reset() for fix.; removed some legacy +++++ comments

Browse Source
This commit is contained in:
Alden Hart
2017-02-13 13:41:23 -05:00
parent e9b6492dbf
commit e7fb4cd672
11 changed files with 39 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/canonical_machine.cpp
+1 -1
View File
@@ -573,7 +573,7 @@ stat_t cm_set_tram(nvObj_t *nv)
// if passed false/0, we will clear the rotation matrix
if (!do_set) {
canonical_machine_reset_rotation(cm); //++++++
canonical_machine_reset_rotation(cm);
return (STAT_OK);
}
g2core/controller.cpp
+2 -2
View File
@@ -201,7 +201,7 @@ static stat_t _dispatch_command()
{
if (cs.controller_state != CONTROLLER_PAUSED) {
devflags_t flags = DEV_IS_BOTH | DEV_IS_MUTED; // expressly state we'll handle muted devices
if ((!mp_planner_is_full(mp)) && (cs.bufp = xio_readline(flags, cs.linelen)) != NULL) { //+++++
if ((!mp_planner_is_full(mp)) && (cs.bufp = xio_readline(flags, cs.linelen)) != NULL) {
_dispatch_kernel(flags);
}
}
@@ -377,7 +377,7 @@ static stat_t _sync_to_tx_buffer()
static stat_t _sync_to_planner()
{
if (mp_planner_is_full(mp)) { // allow up to N planner buffers for this line //+++++
if (mp_planner_is_full(mp)) { // allow up to N planner buffers for this line
return (STAT_EAGAIN);
}
return (STAT_OK);
g2core/cycle_homing.cpp
+1 -1
View File
@@ -174,7 +174,7 @@ stat_t cm_homing_cycle_start(const float axes[], const bool flags[]) {
hm.set_coordinates = true;
// clear rotation matrix
canonical_machine_reset_rotation(cm); //+++++ cleanup
canonical_machine_reset_rotation(cm);
hm.axis = -1; // set to retrieve initial axis
hm.func = _homing_axis_start; // bind initial processing function
g2core/gpio.cpp
+1 -2
View File
@@ -210,8 +210,7 @@ struct ioDigitalInputExt {
cm->safety_interlock_reengaged = ext_pin_number;
}
}
sr_request_status_report(SR_REQUEST_TIMED); //+++++ Put this one back in.
sr_request_status_report(SR_REQUEST_TIMED);
};
};
g2core/plan_arc.cpp
+1 -1
View File
@@ -72,7 +72,7 @@ stat_t cm_arc_callback(cmMachine_t *_cm)
if (_cm->arc.run_state == BLOCK_INACTIVE) {
return (STAT_NOOP);
}
if (mp_planner_is_full(mp)) { //+++++
if (mp_planner_is_full(mp)) {
return (STAT_EAGAIN);
}
_cm->arc.theta += _cm->arc.segment_theta;
g2core/plan_exec.cpp
+2 -2
View File
@@ -36,7 +36,7 @@
#include "report.h"
#include "util.h"
#include "spindle.h"
#include "xio.h" //+++++DIAGNOSTIC
#include "xio.h" // DIAGNOSTIC
// execute routines (NB: These are all called from the LO interrupt)
static stat_t _exec_aline_head(mpBuf_t *bf); // passing bf because body might need it, and it might call body
@@ -494,7 +494,7 @@ stat_t mp_exec_aline(mpBuf_t *bf)
}
else {
// Uh oh! We have a move that's all body, and is still too short!!
// while (1); // ++++ RG For now, we'll consider this impossible.
// while (1); // RG: For now, we'll consider this impossible.
cs.exec_aline_assertion_failure = true;
return (STAT_EXEC_ALINE_ASSERTION_FAILURE);
}
g2core/plan_line.cpp
+4 -4
View File
@@ -55,12 +55,12 @@ static void _calculate_jerk(mpBuf_t* bf);
static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const float axis_square[]);
static void _calculate_junction_vmax(mpBuf_t* bf);
//+++++DIAGNOSTICS
// DIAGNOSTICS
#pragma GCC optimize("O0") // this pragma is required to force the planner to actually set these unused values
//#pragma GCC reset_options
static void _set_bf_diagnostics(mpBuf_t* bf) {
bf->linenum = bf->gm.linenum;
// UPDATE_BF_DIAGNOSTICS(bf); //+++++
// UPDATE_BF_DIAGNOSTICS(bf);
}
#pragma GCC reset_options
@@ -222,7 +222,7 @@ stat_t mp_aline(GCodeState_t* gm_in)
}
_calculate_jerk(bf); // compute bf->jerk values
_calculate_vmaxes(bf, axis_length, axis_square); // compute cruise_vmax and absolute_vmax
_set_bf_diagnostics(bf); //+++++DIAGNOSTIC
_set_bf_diagnostics(bf); // DIAGNOSTIC
// Note: these next lines must remain in exact order. Position must update before committing the buffer.
copy_vector(mp->position, bf->gm.target); // set the planner position
@@ -263,7 +263,7 @@ void mp_plan_block_list()
bf = _plan_block(bf); // returns next block to plan
planned_something = true;
mp->p = bf; //+++++ DIAGNOSTIC - this is not needed but is set here for debugging purposes
mp->p = bf; // DIAGNOSTIC - this is not needed but is set here for debugging purposes
}
if (mp->planner_state > PLANNER_STARTUP) {
if (planned_something && (cm->hold_state != FEEDHOLD_HOLD)) {
g2core/plan_zoid.cpp
+3 -3
View File
@@ -32,7 +32,7 @@
#include "report.h"
#include "util.h"
//+++++ DIAGNOSTICS
// DIAGNOSTICS
//#if IN_DEBUGGER < 1
#define LOG_RETURN(msg) // LOG_RETURN with no action (production)
@@ -60,7 +60,7 @@
}
#endif
//+++++ END DIAGNOSTICS
// END DIAGNOSTICS
/* local functions */
@@ -116,7 +116,7 @@ static float _get_meet_velocity(const float v_0,
void _zoid_exit(mpBuf_t* bf, zoidExitPoint exit_point)
{
//+++++ DIAGNOSTIC
// DIAGNOSTIC
// bf->zoid_exit = exit_point;
if (mp_runtime_is_idle()) { // normally the runtime keeps this value fresh
// bf->time_in_plan_ms += bf->block_time_ms;
g2core/planner.cpp
+17 -19
View File
@@ -62,7 +62,7 @@
#include "report.h"
#include "util.h"
#include "json_parser.h"
#include "xio.h" //+++++ DIAGNOSTIC - only needed if xio_writeline() direct prints are used
#include "xio.h" // DIAGNOSTIC - only needed if xio_writeline() direct prints are used
// Allocate planner structures
@@ -193,18 +193,17 @@ void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uin
memset(_mr, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status
_mr->magic_start = MAGICNUM; // mr assertions
_mr->magic_end = MAGICNUM;
_mr->bf[0].nx = &_mr->bf[1]; // Handle the two "stub blocks" in the runtime structure.
_mr->bf[0].nx = &_mr->bf[1]; // Handle the two "stub blocks" in the runtime structure
_mr->bf[1].nx = &_mr->bf[0];
_mr->r = &_mr->bf[0];
_mr->p = &_mr->bf[1];
}
void planner_reset(mpPlanner_t *_mp) // reset planner queue, cease MR activity, but leave positions alone
void planner_reset(mpPlanner_t *_mp) // reset planner queue, cease MR activity, but leave positions alone
{
// planner_init(_mp, _mp->mr, _mp->q.bf, _mp->q.queue_size); // reset parent planner and linked Q and MR
// _mp->mr.block_state == BLOCK_INACTIVE;
_init_planner_queue(_mp, _mp->q.bf, _mp->q.queue_size);
_mp->mr->block_state = BLOCK_INACTIVE; // this resets the MR structure without actually wiping it
}
stat_t planner_test_assertions(const mpPlanner_t *_mp)
@@ -528,8 +527,7 @@ bool mp_is_phat_city_time()
stat_t mp_planner_callback()
{
// Test if the planner has transitioned to an IDLE state
// if ((mp_get_planner_buffers(mp) == PLANNER_BUFFER_POOL_SIZE) && //+++++ // detect and set IDLE state
if ((mp_get_planner_buffers(mp) == mp->q.queue_size) && //+++++ // detect and set IDLE state
if ((mp_get_planner_buffers(mp) == mp->q.queue_size) && // detect and set IDLE state
(cm->motion_state == MOTION_STOP) &&
(cm->hold_state == FEEDHOLD_OFF)) {
mp->planner_state = PLANNER_IDLE;
@@ -547,11 +545,11 @@ stat_t mp_planner_callback()
// Process a planner request or timeout
if (mp->planner_state == PLANNER_IDLE) {
mp->p = mp_get_r(); //+++++ // initialize planner pointer to run buffer
mp->p = mp_get_r(); // initialize planner pointer to run buffer
mp->planner_state = PLANNER_STARTUP;
}
if (mp->planner_state == PLANNER_STARTUP) {
if (!mp_planner_is_full(mp) && !_timed_out) { //+++++
if (!mp_planner_is_full(mp) && !_timed_out) {
return (STAT_OK); // remain in STARTUP
}
mp->planner_state = PLANNER_PRIMING;
@@ -574,7 +572,7 @@ void mp_replan_queue(mpBuf_t *bf)
} else { // If it's not "planned" then it's either PREPPED or earlier.
break; // We don't need to adjust it.
}
} while ((bf = mp_get_next_buffer(bf)) != mp_get_r()); //+++++
} while ((bf = mp_get_next_buffer(bf)) != mp_get_r());
mp->request_planning = true;
}
@@ -641,20 +639,20 @@ void mp_end_traverse_override(const float ramp_time)
void mp_planner_time_accounting()
{
mpBuf_t *bf = mp_get_r(); //+++++ // start with run buffer
mpBuf_t *bf = mp_get_r(); // start with run buffer
// check the run buffer to see if anything is running. Might not be
if (bf->buffer_state != MP_BUFFER_RUNNING) { // this is not an error condition
return;
}
mp->plannable_time = 0; //UPDATE_BF_MS(bf); //+++++
while ((bf = bf->nx) != mp_get_r()) { //+++++
mp->plannable_time = 0; //UPDATE_BF_MS(bf); // DIAGNOSTIC
while ((bf = bf->nx) != mp_get_r()) {
if (bf->buffer_state == MP_BUFFER_EMPTY || bf->plannable == true) {
break;
}
mp->plannable_time += bf->block_time;
}
UPDATE_MP_DIAGNOSTICS //+++++
UPDATE_MP_DIAGNOSTICS // DIAGNOSTIC
}
/**** PLANNER BUFFER PRIMITIVES ************************************************************
@@ -745,8 +743,8 @@ mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) { return (bf->pv); }
mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) { return (bf->nx); }
*/
mpBuf_t * mp_get_w() { return (mp->q.w); } //++++ should this be a DI function?
mpBuf_t * mp_get_r() { return (mp->q.r); } //+++++ ditto
mpBuf_t * mp_get_w() { return (mp->q.w); }
mpBuf_t * mp_get_r() { return (mp->q.r); }
mpBuf_t * mp_get_write_buffer() // get & clear a buffer
{
@@ -754,7 +752,7 @@ mpBuf_t * mp_get_write_buffer() // get & clear a buffer
mpPlannerQueue_t *q = &(mp->q);
if (q->w->buffer_state == MP_BUFFER_EMPTY) {
_clear_buffer(q->w); // ++++RG this is redundant, it was just cleared in mp_free_run_buffer
_clear_buffer(q->w); // RG: this is redundant, it was just cleared in mp_free_run_buffer
q->w->buffer_state = MP_BUFFER_INITIALIZING;
q->buffers_available--;
return (mp_get_w());
@@ -828,7 +826,7 @@ bool mp_free_run_buffer() // EMPTY current run buffer & advance to the
mpBuf_t *r_now = q->r; // save this pointer is to avoid a race condition when clearing the buffer
_audit_buffers(); // ++++diagnostic audit for buffer chain integrity (only runs in DEBUG mode)
_audit_buffers(); // DIAGNOSTIC audit for buffer chain integrity (only runs in DEBUG mode)
q->r = q->r->nx; // advance to next run buffer first,
_clear_buffer(r_now); // ... then clear out the old buffer (& set MP_BUFFER_EMPTY)
g2core/planner.h
+4 -4
View File
@@ -309,19 +309,19 @@ typedef struct mpBuffer {
// *** CAUTION *** These two pointers are not reset by _clear_buffer()
struct mpBuffer *pv; // static pointer to previous buffer
struct mpBuffer *nx; // static pointer to next buffer
uint8_t buffer_number; //+++++ DIAGNOSTIC for easier debugging
uint8_t buffer_number; // DIAGNOSTIC for easier debugging
stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function
cm_exec_t cm_func; // callback to canonical machine execution function
//+++++ DIAGNOSTICS for easier debugging
// DIAGNOSTICS for easier debugging...
uint32_t linenum; // mirror of bf->gm.linenum
int iterations;
float block_time_ms;
float plannable_time_ms; // time in planner
float plannable_length; // length in planner
uint8_t meet_iterations; // iterations needed in _get_meet_velocity
//+++++ to here
//...to here
bufferState buffer_state; // used to manage queuing/dequeuing
blockType block_type; // used to dispatch to run routine
@@ -480,7 +480,7 @@ typedef struct mpPlannerRuntime { // persistent runtime variables
typedef struct mpPlanner { // common variables for a planner context
magic_t magic_start; // magic number to test memory integrity
//+++++ DIAGNOSTICS
// DIAGNOSTICS
float run_time_remaining_ms;
float plannable_time_ms;
g2core/report.cpp
+3 -3
View File
@@ -2,7 +2,7 @@
* report.cpp - Status reports and other reporting functions
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2016 Alden S. Hart, Jr.
* Copyright (c) 2010 - 2017 Alden S. Hart, Jr.
*
* This file ("the software") is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License, version 2 as published by the
@@ -487,7 +487,7 @@ void qr_init_queue_report()
void qr_request_queue_report(int8_t buffers)
{
// get buffer depth and added/removed count
qr.buffers_available = mp_get_planner_buffers(mp); //+++++
qr.buffers_available = mp_get_planner_buffers(mp);
if (buffers > 0) {
qr.buffers_added += buffers;
} else {
@@ -571,7 +571,7 @@ stat_t qr_queue_report_callback() // called by controller dispatcher
*/
stat_t qr_get(nvObj_t *nv)
{
nv->value = (float)mp_get_planner_buffers(mp); //+++++ // ensure that manually requested QR count is always up to date
nv->value = (float)mp_get_planner_buffers(mp); // ensure that manually requested QR count is always up to date
nv->valuetype = TYPE_INT;
return (STAT_OK);
}