diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index ecfa0264..8c5e050f 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -135,6 +135,100 @@ static int8_t _axis(const index_t index); **** CODE ************************************************************************* ***********************************************************************************/ +/* + * canonical_machine_init() - initialize cm struct + * canonical_machine_reset() - apply startup settings or reset to startup + * canonical_machine_reset_rotation() + */ + +void canonical_machine_init(cmMachine_t *_cm) +{ + // Note cm* was assignd in main() + // If you can assume all memory has been zeroed by a hard reset you don't need this code: + memset(_cm, 0, sizeof(cmMachine_t)); // do not reset canonicalMachine once it's been initialized + memset(&_cm->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status + + canonical_machine_init_assertions(_cm); // establish assertions + ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer + cm_arc_init(_cm); // Note: spindle and coolant inits are independent +} + +// Note: run canonical_machine_init and profile initializations beforehand +void canonical_machine_reset(cmMachine_t *_cm) +{ + // reset canonical machine assertions + canonical_machine_init_assertions(_cm); + + // set canonical machine gcode defaults + cm_set_units_mode(gc.default_units_mode); + cm_set_coord_system(gc.default_coord_system); // NB: queues a block to the planner with the coordinates + cm_select_plane(gc.default_select_plane); + cm_set_path_control(MODEL, gc.default_path_control); + cm_set_distance_mode(gc.default_distance_mode); + cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default + cm_reset_overrides(); // set overrides to initial conditions + + // NOTE: Should unhome axes here + + // reset request flags + _cm->queue_flush_state = FLUSH_OFF; + _cm->end_hold_requested = false; + _cm->limit_requested = 0; // resets switch closures that occurred during initialization + _cm->safety_interlock_disengaged = 0; // ditto + _cm->safety_interlock_reengaged = 0; // ditto + _cm->shutdown_requested = 0; // ditto + + // set initial state and signal that the machine is ready for action + _cm->cycle_state = CYCLE_OFF; + _cm->motion_state = MOTION_STOP; + _cm->hold_state = FEEDHOLD_OFF; + _cm->esc_boot_timer = SysTickTimer_getValue(); + _cm->gmx.block_delete_switch = true; + _cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode + _cm->machine_state = MACHINE_READY; + + canonical_machine_reset_rotation(_cm); + memset(&_cm->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED); + memset(&_cm->probe_results, 0, sizeof(float)*PROBES_STORED*AXES); +} + +void canonical_machine_reset_rotation(cmMachine_t *_cm) { + + // We must make it an identity matrix for no rotation + memset(&_cm->rotation_matrix, 0, sizeof(float)*3*3); + _cm->rotation_matrix[0][0] = 1.0; + _cm->rotation_matrix[1][1] = 1.0; + _cm->rotation_matrix[2][2] = 1.0; + _cm->rotation_z_offset = 0.0; +} + +/* + * canonical_machine_init_assertions() + * canonical_machine_test_assertions() - test assertions, return error code if violation exists + */ + +void canonical_machine_init_assertions(cmMachine_t *_cm) +{ + _cm->magic_start = MAGICNUM; + _cm->magic_end = MAGICNUM; + _cm->gmx.magic_start = MAGICNUM; + _cm->gmx.magic_end = MAGICNUM; + _cm->arc.magic_start = MAGICNUM; + _cm->arc.magic_end = MAGICNUM; +} + +stat_t canonical_machine_test_assertions(cmMachine_t *_cm) +{ + if ((BAD_MAGIC(_cm->magic_start)) || (BAD_MAGIC(_cm->magic_end)) || + (BAD_MAGIC(_cm->gmx.magic_start)) || (BAD_MAGIC(_cm->gmx.magic_end)) || + (BAD_MAGIC(_cm->arc.magic_start)) || (BAD_MAGIC(_cm->arc.magic_end))) { + return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()")); + } + return (STAT_OK); +} + + /************************************* * Internal getters and setters * * Canonical Machine State functions * @@ -713,103 +807,7 @@ stat_t cm_test_soft_limits(const float target[]) * found in NIST RS274 NGCv3 ************************************************************************/ -/****************************************** - * Initialization and Termination (4.3.2) * - ******************************************/ -/* - * canonical_machine_init() - initialize cm struct - * canonical_machine_reset() - apply startup settings or reset to startup - * run profile initialization beforehand - */ -void canonical_machine_init(cmMachine_t *m) -{ - // Note cm* was assignd in main() - // If you can assume all memory has been zeroed by a hard reset you don't need this code: - memset(m, 0, sizeof(cmMachine_t)); // do not reset canonicalMachine once it's been initialized -// memset(&m->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status - -// memset(&gc.gn, 0, sizeof(GCodeInput_t)); // reset the Gcode inputs -// memset(&gc.gf, 0, sizeof(GCodeFlags_t)); - - canonical_machine_init_assertions(m); // establish assertions - ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - cm_arc_init(); // Note: spindle and coolant inits are independent -} - -void canonical_machine_reset(cmMachine_t *m) -{ - // reset canonical machine assertions - canonical_machine_init_assertions(m); - - // set gcode defaults - cm_set_units_mode(gc.default_units_mode); - cm_set_coord_system(gc.default_coord_system); // NB: queues a block to the planner with the coordinates - cm_select_plane(gc.default_select_plane); - cm_set_path_control(MODEL, gc.default_path_control); - cm_set_distance_mode(gc.default_distance_mode); - cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default - cm_reset_overrides(); // set overrides to initial conditions - - // NOTE: Should unhome axes here - - // reset request flags - m->queue_flush_state = FLUSH_OFF; - m->end_hold_requested = false; - m->limit_requested = 0; // resets switch closures that occurred during initialization - m->safety_interlock_disengaged = 0; // ditto - m->safety_interlock_reengaged = 0; // ditto - m->shutdown_requested = 0; // ditto - - // set initial state and signal that the machine is ready for action - m->cycle_state = CYCLE_OFF; - m->motion_state = MOTION_STOP; - m->hold_state = FEEDHOLD_OFF; - m->esc_boot_timer = SysTickTimer_getValue(); - m->gmx.block_delete_switch = true; - m->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode - m->machine_state = MACHINE_READY; - - canonical_machine_reset_rotation(m); - memset(&m->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED); - memset(&m->probe_results, 0, sizeof(float)*PROBES_STORED*AXES); -} - -void canonical_machine_reset_rotation(cmMachine_t *m) { - memset(&cm->rotation_matrix, 0, sizeof(float)*3*3); - - // We must make it an identity matrix for no rotation - m->rotation_matrix[0][0] = 1.0; - m->rotation_matrix[1][1] = 1.0; - m->rotation_matrix[2][2] = 1.0; - m->rotation_z_offset = 0.0; -} - -/* - * canonical_machine_init_assertions() - * canonical_machine_test_assertions() - test assertions, return error code if violation exists - */ - -void canonical_machine_init_assertions(cmMachine_t *m) -{ - m->magic_start = MAGICNUM; - m->magic_end = MAGICNUM; - m->gmx.magic_start = MAGICNUM; - m->gmx.magic_end = MAGICNUM; - m->arc.magic_start = MAGICNUM; - m->arc.magic_end = MAGICNUM; -} - -stat_t canonical_machine_test_assertions(cmMachine_t *m) -{ - if ((BAD_MAGIC(m->magic_start)) || (BAD_MAGIC(m->magic_end)) || - (BAD_MAGIC(m->gmx.magic_start)) || (BAD_MAGIC(m->gmx.magic_end)) || - (BAD_MAGIC(m->arc.magic_start)) || (BAD_MAGIC(m->arc.magic_end))) { - return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()")); - } - return (STAT_OK); -} /************************** * Alarms * diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 24c1da8d..25d9da0c 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -340,11 +340,11 @@ stat_t cm_test_soft_limits(const float target[]); /*--- Canonical machining functions (loosely) defined by NIST [organized by NIST Gcode doc] ---*/ // Initialization and termination (4.3.2) -void canonical_machine_init(cmMachine_t *machine); -void canonical_machine_reset_rotation(cmMachine_t *machine); // NOT in NIST -void canonical_machine_reset(cmMachine_t *machine); -void canonical_machine_init_assertions(cmMachine_t *machine); -stat_t canonical_machine_test_assertions(cmMachine_t *machine); +void canonical_machine_init(cmMachine_t *_cm); +void canonical_machine_reset_rotation(cmMachine_t *_cm); // NOT in NIST +void canonical_machine_reset(cmMachine_t *_cm); +void canonical_machine_init_assertions(cmMachine_t *_cm); +stat_t canonical_machine_test_assertions(cmMachine_t *_cm); // Alarms and state management stat_t cm_alrm(nvObj_t *nv); // trigger alarm from command input diff --git a/g2core/controller.cpp b/g2core/controller.cpp index cdae6602..553f2878 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -2,8 +2,8 @@ * controller.cpp - g2core controller and top level parser * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2017 Robert Giseburt * * 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 @@ -161,7 +161,7 @@ static void _controller_HSM() DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner DISPATCH(mp_planner_callback()); // motion planner - DISPATCH(cm_arc_callback()); // arc generation runs as a cycle above lines + DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines DISPATCH(cm_homing_cycle_callback()); // homing cycle operation (G28.2) DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2) DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation diff --git a/g2core/gcode.h b/g2core/gcode.h index 94917883..8e24f1e0 100644 --- a/g2core/gcode.h +++ b/g2core/gcode.h @@ -386,6 +386,7 @@ extern GCodeSingleton_t gc; // gcode structs /* * Global Scope Functions */ +void gcode_parser_init(void); stat_t gcode_parser(char* block); stat_t gc_get_gc(nvObj_t* nv); stat_t gc_run_gc(nvObj_t* nv); diff --git a/g2core/gcode_parser.cpp b/g2core/gcode_parser.cpp index c26337ec..f87c7594 100644 --- a/g2core/gcode_parser.cpp +++ b/g2core/gcode_parser.cpp @@ -2,8 +2,8 @@ * gcode_parser.cpp - rs274/ngc Gcode parser * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Rob Giseburt + * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2017 Rob Giseburt * * 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 @@ -41,6 +41,16 @@ static stat_t _execute_gcode_block(char *active_comment); // Execute t #define SET_NON_MODAL(parm,val) ({gc.gn.parm=val; gc.gf.parm=true; break;}) #define EXEC_FUNC(f,v) if(gc.gf.v) { status=f(gc.gn.v);} +/* + * gcode_parser_init() + */ + +void gcode_parser_init() +{ + memset(&gc.gn, 0, sizeof(GCodeInput_t)); // reset the Gcode inputs + memset(&gc.gf, 0, sizeof(GCodeFlags_t)); +} + /* * gcode_parser() - parse a block (line) of gcode * diff --git a/g2core/main.cpp b/g2core/main.cpp index f48c9df7..a00cccd0 100644 --- a/g2core/main.cpp +++ b/g2core/main.cpp @@ -108,10 +108,11 @@ void application_init_machine(void) encoder_init(); // virtual encoders gpio_init(); // inputs and outputs pwm_init(); // pulse width modulation drivers - planner_init(&mp1, &mr1, mp1_pool, PLANNER_BUFFER_POOL_SIZE); // primary motion planner - planner_init(&mp2, &mr2, mp2_pool, SECONDARY_BUFFER_POOL_SIZE); // secondary motion planner + + planner_init(&mp1, &mr1, mp1_queue, PLANNER_QUEUE_SIZE); + planner_init(&mp2, &mr2, mp2_queue, SECONDARY_QUEUE_SIZE); canonical_machine_init(&cm1); // primary canonical machine -// canonical_machine_init(&cm2); // secondary canonical machine + canonical_machine_init(&cm2); // secondary canonical machine } void application_init_startup(void) @@ -119,8 +120,8 @@ void application_init_startup(void) // start the application controller_init(); // should be first startup init (requires xio_init()) config_init(); // apply the config settings from persistence - canonical_machine_reset(&cm1); -// canonical_machine_reset(&cm2); + canonical_machine_reset(&cm1); // initialize both CMs but only reset the primary + gcode_parser_init(); // baseline Gcode parser spindle_init(); // should be after PWM and canonical machine inits and config_init() spindle_reset(); temperature_init(); diff --git a/g2core/plan_arc.cpp b/g2core/plan_arc.cpp index 944e3925..99403d63 100644 --- a/g2core/plan_arc.cpp +++ b/g2core/plan_arc.cpp @@ -2,7 +2,7 @@ * plan_arc.c - arc planning and motion execution * 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 @@ -47,10 +47,10 @@ static stat_t _test_arc_soft_limits(void); /* * cm_arc_init() - initialize arc structures */ -void cm_arc_init() +void cm_arc_init(cmMachine_t *_cm) { - cm->arc.magic_start = MAGICNUM; - cm->arc.magic_end = MAGICNUM; + _cm->arc.magic_start = MAGICNUM; + _cm->arc.magic_end = MAGICNUM; } /* @@ -59,9 +59,9 @@ void cm_arc_init() * OK to call if no arc is running */ -void cm_abort_arc() +void cm_abort_arc(cmMachine_t *_cm) { - cm->arc.run_state = BLOCK_INACTIVE; + _cm->arc.run_state = BLOCK_INACTIVE; } /* @@ -73,26 +73,26 @@ void cm_abort_arc() * Parts of this routine were informed by the grbl project. */ -stat_t cm_arc_callback() +stat_t cm_arc_callback(cmMachine_t *_cm) { - if (cm->arc.run_state == BLOCK_INACTIVE) { + if (_cm->arc.run_state == BLOCK_INACTIVE) { return (STAT_NOOP); } if (mp_planner_is_full(mp)) { //+++++ return (STAT_EAGAIN); } - cm->arc.theta += cm->arc.segment_theta; - cm->arc.gm.target[cm->arc.plane_axis_0] = cm->arc.center_0 + sin(cm->arc.theta) * cm->arc.radius; - cm->arc.gm.target[cm->arc.plane_axis_1] = cm->arc.center_1 + cos(cm->arc.theta) * cm->arc.radius; - cm->arc.gm.target[cm->arc.linear_axis] += cm->arc.segment_linear_travel; + _cm->arc.theta += _cm->arc.segment_theta; + _cm->arc.gm.target[_cm->arc.plane_axis_0] = _cm->arc.center_0 + sin(_cm->arc.theta) * _cm->arc.radius; + _cm->arc.gm.target[_cm->arc.plane_axis_1] = _cm->arc.center_1 + cos(_cm->arc.theta) * _cm->arc.radius; + _cm->arc.gm.target[_cm->arc.linear_axis] += _cm->arc.segment_linear_travel; - mp_aline(&(cm->arc.gm)); // run the line - copy_vector(cm->arc.position, cm->arc.gm.target); // update arc current position + mp_aline(&(_cm->arc.gm)); // run the line + copy_vector(_cm->arc.position, _cm->arc.gm.target); // update arc current position - if (--(cm->arc.segment_count) > 0) { + if (--(_cm->arc.segment_count) > 0) { return (STAT_EAGAIN); } - cm->arc.run_state = BLOCK_INACTIVE; + _cm->arc.run_state = BLOCK_INACTIVE; return (STAT_OK); } diff --git a/g2core/plan_arc.h b/g2core/plan_arc.h index 5e2f0dc8..641fb86d 100644 --- a/g2core/plan_arc.h +++ b/g2core/plan_arc.h @@ -2,7 +2,7 @@ * plan_arc.h - arc planning and motion execution * 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 @@ -31,49 +31,11 @@ #define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test #define CHORDAL_TOLERANCE_MIN (0.001) // values below this are not accepted -/* -typedef struct arArcSingleton { // persistent planner and runtime variables - magic_t magic_start; - uint8_t run_state; // runtime state machine sequence - float position[AXES]; // accumulating runtime position - float offset[3]; // arc IJK offsets - - float length; // length of line or helix in mm - float radius; // Raw R value, or computed via offsets - float theta; // starting angle of arc - float angular_travel; // travel along the arc in radians - float planar_travel; // travel in arc plane in mm - float linear_travel; // travel along linear axis of arc in mm - bool full_circle; // True if full circle arcs specified - float rotations; // number of full rotations to add (P value + sign) - - cmAxes plane_axis_0; // arc plane axis 0 - e.g. X for G17 - cmAxes plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - cmAxes linear_axis; // linear axis (normal to plane) - - float segments; // number of segments in arc or blend - int32_t segment_count; // count of running segments - float segment_theta; // angular motion per segment - float segment_linear_travel; // linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - - GCodeState_t gm; // Gcode state struct is passed for each arc segment. - // Usage: - // uint32_t linenum; // line number of the arc feed move - same for each segment - // float target[AXES]; // arc segment target -// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment) -// float block_time; // segment_time: constant time per aline segment - - magic_t magic_end; -} arc_t; -extern arc_t arc; -*/ /* arc function prototypes */ -void cm_arc_init(void); -void cm_abort_arc(void); -stat_t cm_arc_callback(void); +void cm_arc_init(cmMachine_t *_cm); +void cm_abort_arc(cmMachine_t *_cm); +stat_t cm_arc_callback(cmMachine_t *_cm); #endif // End of include guard: PLAN_ARC_H_ONCE diff --git a/g2core/planner.cpp b/g2core/planner.cpp index da61dd42..183c61be 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -74,8 +74,8 @@ mpPlannerRuntime_t *mr; // context for planner block runtime mpPlannerRuntime_t mr1; // primary planner runtime context mpPlannerRuntime_t mr2; // secondary planner runtime context -mpBuf_t mp1_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers -mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers +mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers +mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers #define JSON_COMMAND_BUFFER_SIZE 3 @@ -147,7 +147,7 @@ static stat_t _exec_json_wait(mpBuf_t *bf); */ // initialize a planner queue -void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size) +void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *queue, uint8_t size) { mpBuf_t *pv, *nx; uint8_t i, nx_i; @@ -157,10 +157,10 @@ void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size) q->magic_start = MAGICNUM; q->magic_end = MAGICNUM; - memset(pool, 0, sizeof(mpBuf_t)*size); // clear all buffers in pool - q->bf = pool; // link the buffer pool first - q->w = pool; // init all buffer pointers - q->r = pool; + memset(queue, 0, sizeof(mpBuf_t)*size); // clear all buffers in queue + q->bf = queue; // link the buffer pool first + q->w = queue; // init all buffer pointers + q->r = queue; q->queue_size = size; q->buffers_available = size; @@ -173,10 +173,10 @@ void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size) q->bf[i].pv = pv; pv = &q->bf[i]; } - q->bf[size-1].nx = pool; + q->bf[size-1].nx = queue; } -void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size) +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size) { mp = &mp1; // set global pointer to the primary planner mr = &mr1; // and primary runtime @@ -188,8 +188,8 @@ void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uin _mp->mfo_factor = 1.00; // init planner queues - _mp->q.bf = _pool; // assign puffer pool to queue manager structure - _init_planner_queue(_mp, _pool, _queue_size); + _mp->q.bf = queue; // assign puffer pool to queue manager structure + _init_planner_queue(_mp, queue, queue_size); // init runtime structs _mp->mr = _mr; @@ -207,22 +207,19 @@ void planner_reset(mpPlanner_t *_mp) planner_init(_mp, _mp->mr, _mp->q.bf, _mp->q.queue_size); // reset parent planner and linked Q and MR } -stat_t planner_test_assertions(mpPlanner_t *_mp) +stat_t planner_test_assertions(const mpPlanner_t *_mp) { if ( (BAD_MAGIC(_mp->magic_start)) || (BAD_MAGIC(_mp->magic_end)) || (BAD_MAGIC(_mp->mr->magic_start)) || (BAD_MAGIC(_mp->mr->magic_end)) ) { - return(cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()")); + return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()")); + } + for (uint8_t i=0; i < _mp->q.queue_size; i++) { + if ((_mp->q.bf[i].nx == nullptr) || (_mp->q.bf[i].pv == nullptr)) { + return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner buffer is corrupted")); + } } -// for (uint8_t i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { -// if (mb.bf[i].nx == nullptr) { -// _debug_trap("buffer has nullptr for nx"); -// } -// if (mb.bf[i].pv == nullptr) { -// _debug_trap("buffer has nullptr for pv"); -// } -// } return (STAT_OK); } @@ -245,9 +242,7 @@ void mp_halt_runtime() */ void mp_flush_planner(mpPlanner_t *_mp) { - cm_abort_arc(); -// mp_init_planner_buffers(_mp); //+++++ -// planner_init(_mp); //+++++ + cm_abort_arc(cm); planner_reset(_mp); mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion } @@ -485,38 +480,19 @@ stat_t mp_exec_out_of_band_dwell(void) * mp_has_runnable_buffer() - true if next buffer is runnable, indicating motion has not stopped. * mp_is_it_phat_city_time() - test if there is time for non-essential processes */ -/* -uint8_t mp_get_planner_buffers(int8_t q) // which queue are you interested in? -{ - if (q == ACTIVE_Q) { q = mb.active_q; }; - return (mb.q[q].buffers_available); -} -bool mp_planner_is_full(int8_t q) // which queue are you interested in? -{ - if (q == ACTIVE_Q) { q = mb.active_q; }; - return ((mb.q[q].buffers_available < PLANNER_BUFFER_HEADROOM) - || (jc.available == 0)); // We also need to ensure we have room for another JSON command -} - -bool mp_has_runnable_buffer(int8_t q) // which queue are you interested in?) -{ - if (q == ACTIVE_Q) { q = mb.active_q; }; - return (mb.q[q].r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true -} -*/ -uint8_t mp_get_planner_buffers(mpPlanner_t *_mp) // which planner are you interested in? +uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp) // which planner are you interested in? { return (_mp->q.buffers_available); } -bool mp_planner_is_full(mpPlanner_t *_mp) // which planner are you interested in? +bool mp_planner_is_full(const mpPlanner_t *_mp) // which planner are you interested in? { // We also need to ensure we have room for another JSON command return ((_mp->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); } -bool mp_has_runnable_buffer(mpPlanner_t *_mp) // which planner are you interested in?) +bool mp_has_runnable_buffer(const mpPlanner_t *_mp) // which planner are you interested in?) { return (_mp->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true } @@ -566,7 +542,8 @@ 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) == PLANNER_BUFFER_POOL_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; diff --git a/g2core/planner.h b/g2core/planner.h index 130a717f..9044aeaa 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -250,8 +250,8 @@ typedef enum { */ /*** Most of these factors are the result of a lot of tweaking. Change with caution.***/ -#define PLANNER_BUFFER_POOL_SIZE ((uint8_t)48) // Suggest 12 min. Limit is 255 -#define SECONDARY_BUFFER_POOL_SIZE ((uint8_t)12) // Secondary planner queue for feedhold operations +#define PLANNER_QUEUE_SIZE ((uint8_t)48) // Suggest 12 min. Limit is 255 +#define SECONDARY_QUEUE_SIZE ((uint8_t)12) // Secondary planner queue for feedhold operations #define PLANNER_BUFFER_HEADROOM ((uint8_t)4) // Buffers to reserve in planner before processing new input line #define JERK_MULTIPLIER ((float)1000000) // DO NOT CHANGE - must always be 1 million @@ -536,8 +536,8 @@ extern mpPlannerRuntime_t *mr; // context for block runtime extern mpPlannerRuntime_t mr1; // primary planner runtime context extern mpPlannerRuntime_t mr2; // secondary planner runtime context -extern mpBuf_t mp1_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers -extern mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers +extern mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers +extern mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers /* * Global Scope Functions @@ -545,9 +545,9 @@ extern mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for s //**** planner.cpp functions -void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size); +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size); void planner_reset(mpPlanner_t *_mp); -stat_t planner_test_assertions(mpPlanner_t *_mp); +stat_t planner_test_assertions(const mpPlanner_t *_mp); void mp_halt_runtime(void); void mp_flush_planner(mpPlanner_t *_mp); @@ -567,9 +567,9 @@ void mp_request_out_of_band_dwell(float seconds); stat_t mp_exec_out_of_band_dwell(void); //**** planner functions and helpers -uint8_t mp_get_planner_buffers(mpPlanner_t *_mp); -bool mp_planner_is_full(mpPlanner_t *_mp); -bool mp_has_runnable_buffer(mpPlanner_t *_mp); +uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp); +bool mp_planner_is_full(const mpPlanner_t *_mp); +bool mp_has_runnable_buffer(const mpPlanner_t *_mp); bool mp_is_phat_city_time(void); stat_t mp_planner_callback();