diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 18d6e778..5cb0b79b 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -113,10 +113,10 @@ **** STRUCTURE ALLOCATIONS ******************************************************** ***********************************************************************************/ -cmCanonicalMachine_t *cm; // pointer to active canonical machine -cmCanonicalMachine_t cm0; // canonical machine primary machine -cmCanonicalMachine_t cm1; // canonical machine secondary machine -cmToolTable_t tt; // global tool table +cmMachine_t *cm; // pointer to active canonical machine +cmMachine_t cm0; // canonical machine primary machine +cmMachine_t cm1; // canonical machine secondary machine +cmToolTable_t tt; // global tool table /*********************************************************************************** **** GENERIC STATIC FUNCTIONS AND VARIABLES *************************************** @@ -488,7 +488,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(); + canonical_machine_reset_rotation(cm); //++++++ cleanup on aisle 5 return (STAT_OK); } @@ -722,30 +722,22 @@ stat_t cm_test_soft_limits(const float target[]) * run profile initialization beforehand */ -void canonical_machine_init() +void canonical_machine_init(cmMachine_t *m) { // NoteL 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(cm0)); // do not reset canonicalMachineSingleton once it's been initialized - memset(&cm->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status - memset(&gc.gn, 0, sizeof(GCodeInput_t)); + 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(); // establish assertions + 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_rotation() { - memset(&cm->rotation_matrix, 0, sizeof(float)*3*3); - // We must make it an identity matrix for no rotation - 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; -} - -void canonical_machine_reset() +void canonical_machine_reset(cmMachine_t *m) { // set gcode defaults cm_set_units_mode(gc.default_units_mode); @@ -760,26 +752,35 @@ void canonical_machine_reset() // 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 + 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 - 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; + 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(); + canonical_machine_reset_rotation(m); + memset(&m->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED); + memset(&m->probe_results, 0, sizeof(float)*PROBES_STORED*AXES); +} - 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 *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; } /* @@ -787,21 +788,21 @@ void canonical_machine_reset() * canonical_machine_test_assertions() - test assertions, return error code if violation exists */ -void canonical_machine_init_assertions(void) +void canonical_machine_init_assertions(cmMachine_t *m) { - 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; + 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(void) +stat_t canonical_machine_test_assertions(cmMachine_t *m) { - 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))) { + 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); @@ -904,8 +905,8 @@ void cm_halt_all(void) void cm_halt_motion(void) { mp_halt_runtime(); // stop the runtime. Do this immediately. (Reset is in cm_clear) - canonical_machine_reset(); // reset Gcode model - cm->cycle_state = CYCLE_OFF; // Note: leaves machine_state alone + canonical_machine_reset(cm); //++++++ // reset Gcode model + cm->cycle_state = CYCLE_OFF; // Note: leaves machine_state alone cm->motion_state = MOTION_STOP; cm->hold_state = FEEDHOLD_OFF; } @@ -1347,12 +1348,12 @@ stat_t cm_straight_traverse(const float target[], const bool flags[]) } cm_set_model_target(target, flags); - ritorno (cm_test_soft_limits(cm->gm.target)); // test soft limits; exit if thrown - cm_set_work_offsets(&cm->gm); // capture the fully resolved offsets to the state + ritorno (cm_test_soft_limits(cm->gm.target)); // test soft limits; exit if thrown + cm_set_work_offsets(&cm->gm); // capture the fully resolved offsets to the state cm_cycle_start(); // required for homing & other cycles - stat_t status = mp_aline(&cm->gm); // send the move to the planner + stat_t status = mp_aline(&cm->gm); // send the move to the planner cm_finalize_move(); - if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(ACTIVE_Q)) { + if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //+++++ cm_cycle_end(); return (STAT_OK); } @@ -1372,7 +1373,7 @@ stat_t _goto_stored_position(const float stored_position[], // always in mm const bool flags[]) // all false if no intermediate move { // Go through intermediate point if one is provided - while (mp_planner_is_full(ACTIVE_Q)); // Make sure you have available buffers + while (mp_planner_is_full(mp)); //+++++ // Make sure you have available buffers ritorno(cm_straight_traverse(intermediate_target, flags)); // w/no action if no axis flags // If G20 adjust stored position (always in mm) to inches so traverse will be correct @@ -1385,7 +1386,7 @@ stat_t _goto_stored_position(const float stored_position[], // always in mm } // Run the stored position move - while (mp_planner_is_full(ACTIVE_Q)); // Make sure you have available buffers + while (mp_planner_is_full(mp)); //+++++ // Make sure you have available buffers uint8_t saved_distance_mode = cm_get_distance_mode(MODEL); cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON); // Position was stored in absolute coords @@ -1510,7 +1511,7 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) cm_finalize_move(); // <-- ONLY safe because we don't care about status... - if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(ACTIVE_Q)) { + if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //+++++ cm_cycle_end(); return (STAT_OK); } @@ -1839,7 +1840,7 @@ bool cm_has_hold() void cm_start_hold() { - if (mp_has_runnable_buffer(ACTIVE_Q)) { // meaning there's something running + if (mp_has_runnable_buffer(mp)) { //+++++ // meaning there's something running cm_spindle_optional_pause(spindle.pause_on_hold); // pause if this option is selected cm_coolant_optional_pause(coolant.pause_on_hold); // pause if this option is selected cm_set_motion_state(MOTION_HOLD); @@ -1875,7 +1876,7 @@ void cm_end_hold() void cm_queue_flush() { if (mp_runtime_is_idle()) { // can't flush planner during movement - mp_flush_planner(); + mp_flush_planner(mp); // +++++ Active planner. Potential cleanup for (uint8_t axis = AXIS_X; axis < AXES; axis++) { // set all positions cm_set_position(axis, mp_get_runtime_absolute_position(axis)); diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 2ba7b699..fddcaa82 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -207,7 +207,7 @@ typedef struct arArcSingleton { // persistent planner and runtim magic_t magic_end; } arc_t; -typedef struct cmCanonicalMachine { // struct to manage canonical machine globals and state +typedef struct cmMachine { // struct to manage canonical machine globals and state magic_t magic_start; // magic number to test memory integrity /**** Config variables (PUBLIC) ****/ @@ -269,7 +269,7 @@ typedef struct cmCanonicalMachine { // struct to manage canonical machin GCodeStateX_t gmx; // extended gcode model state magic_t magic_end; -} cmCanonicalMachine_t; +} cmMachine_t; typedef struct cmToolTable { // struct to keep a global tool table float tt_offset[TOOLS+1][AXES]; // persistent tool table offsets @@ -277,9 +277,9 @@ typedef struct cmToolTable { // struct to keep a global tool tabl /**** Externs - See canonical_machine.cpp for allocation ****/ -extern cmCanonicalMachine_t *cm; // pointer to active canonical machine -extern cmCanonicalMachine_t cm0; // canonical machine primary machine -extern cmCanonicalMachine_t cm1; // canonical machine secondary machine +extern cmMachine_t *cm; // pointer to active canonical machine +extern cmMachine_t cm0; // canonical machine primary machine +extern cmMachine_t cm1; // canonical machine secondary machine extern cmToolTable_t tt; /***************************************************************************** @@ -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(void); -void canonical_machine_reset_rotation(void); // NOT in NIST -void canonical_machine_reset(void); -void canonical_machine_init_assertions(void); -stat_t canonical_machine_test_assertions(void); +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); // 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 87135210..097bb099 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -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(ACTIVE_Q)) && (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(ACTIVE_Q)) { // 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); @@ -475,8 +475,8 @@ stat_t _test_system_assertions() // these functions will panic if an assertion fails _test_assertions(); // controller assertions (local) config_test_assertions(); - canonical_machine_test_assertions(); - planner_test_assertions(); + canonical_machine_test_assertions(&cm0); // +++++ cleanup later + planner_test_assertions(&mp0); // +++++ cleanup later stepper_test_assertions(); encoder_test_assertions(); xio_test_assertions(); diff --git a/g2core/cycle_homing.cpp b/g2core/cycle_homing.cpp index 53542d89..fbec4f6a 100644 --- a/g2core/cycle_homing.cpp +++ b/g2core/cycle_homing.cpp @@ -168,7 +168,7 @@ stat_t cm_homing_cycle_start(void) { hm.set_coordinates = true; // clear rotation matrix - canonical_machine_reset_rotation(); + canonical_machine_reset_rotation(cm); //+++++ cleanup hm.axis = -1; // set to retrieve initial axis hm.func = _homing_axis_start; // bind initial processing function diff --git a/g2core/cycle_jogging.cpp b/g2core/cycle_jogging.cpp index 34b6bb3b..a2f7b4e2 100644 --- a/g2core/cycle_jogging.cpp +++ b/g2core/cycle_jogging.cpp @@ -132,7 +132,7 @@ stat_t cm_jogging_cycle_callback(void) { return (STAT_EAGAIN); // sync to planner move ends } // if (jog.func == _jogging_axis_ramp_jog && mp_get_buffers_available() < PLANNER_BUFFER_HEADROOM) { - if (jog.func == _jogging_axis_ramp_jog && mp_planner_is_full(ACTIVE_Q)) { + if (jog.func == _jogging_axis_ramp_jog && mp_planner_is_full(mp)) { //+++++ return (STAT_EAGAIN); // prevent flooding the queue with jog moves } return (jog.func(jog.axis)); // execute the current jogging move diff --git a/g2core/main.cpp b/g2core/main.cpp index 1535ffe2..b2a487cf 100644 --- a/g2core/main.cpp +++ b/g2core/main.cpp @@ -101,15 +101,18 @@ void application_init_services(void) void application_init_machine(void) { - cm = &cm0; + cm = &cm0; // set global canonical machine pointer to primary machine cm->machine_state = MACHINE_INITIALIZING; stepper_init(); // stepper subsystem encoder_init(); // virtual encoders gpio_init(); // inputs and outputs pwm_init(); // pulse width modulation drivers - planner_init(); // motion planning subsystem - canonical_machine_init(); // canonical machine + runtime_init(); // runtime execution structure + planner_init(&mp0); //+++++ // motion planning subsystem +// planner_init(&mp1); //+++++ // motion planning subsystem + canonical_machine_init(&cm0); //+++++ cleanup required // canonical machine +// canonical_machine_init(&cm1); //+++++ cleanup required // canonical machine } void application_init_startup(void) @@ -117,7 +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(); + canonical_machine_reset(&cm1); // +++++ +// canonical_machine_reset(&cm0); // +++++ 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 5401d0fc..944e3925 100644 --- a/g2core/plan_arc.cpp +++ b/g2core/plan_arc.cpp @@ -78,7 +78,7 @@ stat_t cm_arc_callback() if (cm->arc.run_state == BLOCK_INACTIVE) { return (STAT_NOOP); } - if (mp_planner_is_full(ACTIVE_Q)) { + if (mp_planner_is_full(mp)) { //+++++ return (STAT_EAGAIN); } cm->arc.theta += cm->arc.segment_theta; diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index cbedc0c0..2adbff98 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -564,7 +564,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) mp_free_run_buffer(); } - mp_replan_queue(mp_get_r(ACTIVE_Q)); // make it replan all the blocks + mp_replan_queue(mp_get_r()); // make it replan all the blocks // mp_replan_queue(mb.r); // make it replan all the blocks return (STAT_OK); @@ -654,7 +654,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) } else { mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer (reset) mr.section_state = SECTION_OFF; - mp.run_time_remaining = 0.0; // it's done, so time goes to zero + mp->run_time_remaining = 0.0; // it's done, so time goes to zero mr.entry_velocity = mr.r->exit_velocity; // feed the old exit into the entry. @@ -694,7 +694,7 @@ stat_t mp_plan_feedhold_move() void mp_exit_hold_state() { cm->hold_state = FEEDHOLD_OFF; - if (mp_has_runnable_buffer(ACTIVE_Q)) { + if (mp_has_runnable_buffer(mp)) { //+++++ cm_set_motion_state(MOTION_RUN); sr_request_status_report(SR_REQUEST_IMMEDIATE); } else { @@ -1074,9 +1074,9 @@ static stat_t _exec_aline_segment() } // Update the mb->run_time_remaining -- we know it's missing the current segment's time before it's loaded, that's ok. - mp.run_time_remaining -= mr.segment_time; - if (mp.run_time_remaining < 0) { - mp.run_time_remaining = 0.0; + mp->run_time_remaining -= mr.segment_time; + if (mp->run_time_remaining < 0) { + mp->run_time_remaining = 0.0; } // Call the stepper prep function diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 740b08b7..12a58d05 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -115,7 +115,7 @@ bool mp_get_runtime_busy() } if ((st_runtime_isbusy() == true) || (mr.block_state == BLOCK_ACTIVE) || - (mp_get_r(ACTIVE_Q)->buffer_state > MP_BUFFER_EMPTY)) { + (mp_get_r()->buffer_state > MP_BUFFER_EMPTY)) { return (true); } return (false); @@ -186,7 +186,7 @@ stat_t mp_aline(GCodeState_t* gm_in) target_rotated[5] = gm_in->target[5]; for (uint8_t axis = 0; axis < AXES; axis++) { - axis_length[axis] = target_rotated[axis] - mp.position[axis]; + axis_length[axis] = target_rotated[axis] - mp->position[axis]; if ((flags[axis] = fp_NOT_ZERO(axis_length[axis]))) { // yes, this supposed to be = not == axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; @@ -225,7 +225,7 @@ stat_t mp_aline(GCodeState_t* gm_in) _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 + copy_vector(mp->position, bf->gm.target); // set the planner position mp_commit_write_buffer(BLOCK_TYPE_ALINE); // commit current block (must follow the position update) return (STAT_OK); } @@ -245,8 +245,8 @@ stat_t mp_aline(GCodeState_t* gm_in) void mp_plan_block_list() { - mpBuf_t* bf = mp.p; - bool planned_something = false; + mpBuf_t* bf = mp->p; + bool planned_something = false; while (true) { // unconditional exit condition @@ -256,21 +256,21 @@ void mp_plan_block_list() // OK to replan running buffer during feedhold, but no other times (not supposed to happen) if ((cm->hold_state == FEEDHOLD_OFF) && (bf->buffer_state == MP_BUFFER_RUNNING)) { - mp.p = mp.p->nx; + mp->p = mp->p->nx; return; } 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 (mp->planner_state > PLANNER_STARTUP) { if (planned_something && (cm->hold_state != FEEDHOLD_HOLD)) { st_request_forward_plan(); // start motion if runtime is not already busy } } - mp.p = bf; // update planner pointer + mp->p = bf; // update planner pointer } /* @@ -281,7 +281,7 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) { // First time blocks - set vmaxes for as many blocks as possible (forward loading of priming blocks) // Note: cruise_vmax was computed in _calculate_vmaxes() in aline() - if (mp.planner_state == PLANNER_PRIMING) { + if (mp->planner_state == PLANNER_PRIMING) { // Timings from *here* if (bf->pv->plannable) { @@ -304,8 +304,8 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) if (bf->nx->plannable) { // read in new buffers until EMPTY return (bf->nx); } - mp.planning_return = bf->nx; // where to return after planning is complete - mp.planner_state = PLANNER_BACK_PLANNING; // start backplanning + mp->planning_return = bf->nx; // where to return after planning is complete + mp->planner_state = PLANNER_BACK_PLANNING; // start backplanning } // Backward Planning Pass @@ -314,7 +314,7 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) // Note: Vmax's are already set by the time you get here // Hint options from back-planning: COMMAND_BLOCK, PERFECT_DECELERATION, PERFECT_CRUISE, MIXED_DECELERATION - if (mp.planner_state == PLANNER_BACK_PLANNING) { + if (mp->planner_state == PLANNER_BACK_PLANNING) { // NOTE: We stop when the previous block is no longer plannable. // We will alter the previous block's exit_velocity. float braking_velocity = 0; // we use this to stre the previous entry velocity, start at 0 @@ -434,8 +434,8 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) } // for loop } // exits with bf pointing to a locked or EMPTY block - mp.planner_state = PLANNER_PRIMING; // revert to initial state - return (mp.planning_return); + mp->planner_state = PLANNER_PRIMING; // revert to initial state + return (mp->planning_return); } /***** ALINE HELPERS ***** @@ -455,22 +455,22 @@ static void _calculate_override(mpBuf_t* bf) // execute ramp to adjust cruise v bf->cruise_vmax = bf->override_factor * bf->cruise_vset; // generate ramp term is a ramp is active - if (mp.ramp_active) { - bf->override_factor += mp.ramp_dvdt * bf->block_time; - if (mp.ramp_dvdt > 0) { // positive is an acceleration ramp - if (bf->override_factor > mp.ramp_target) { - bf->override_factor = mp.ramp_target; - mp.ramp_active = false; // detect end of ramp + if (mp->ramp_active) { + bf->override_factor += mp->ramp_dvdt * bf->block_time; + if (mp->ramp_dvdt > 0) { // positive is an acceleration ramp + if (bf->override_factor > mp->ramp_target) { + bf->override_factor = mp->ramp_target; + mp->ramp_active = false; // detect end of ramp } bf->cruise_velocity *= bf->override_factor; if (bf->cruise_velocity > bf->absolute_vmax) { // test max cruise_velocity bf->cruise_velocity = bf->absolute_vmax; - mp.ramp_active = false; // don't allow exceeding absolute_vmax + mp->ramp_active = false; // don't allow exceeding absolute_vmax } } else { // negative is deceleration ramp - if (bf->override_factor < mp.ramp_target) { - bf->override_factor = mp.ramp_target; - mp.ramp_active = false; + if (bf->override_factor < mp->ramp_target) { + bf->override_factor = mp->ramp_target; + mp->ramp_active = false; } bf->cruise_velocity *= bf->override_factor; // +++++ this is probably wrong // bf->exit_velocity *= bf->mfo_factor; //...but I'm not sure this is right, diff --git a/g2core/planner.cpp b/g2core/planner.cpp index ec534c15..d1ec21f3 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -65,10 +65,12 @@ #include "xio.h" //+++++ DIAGNOSTIC - only needed if xio_writeline() direct prints are used // Allocate planner structures -mpBufferQueue_t mb; // buffer pool management -mpBuf_t mb_pool0[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers -mpBuf_t mb_pool1[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers -mpMotionPlannerHead_t mp; // context for block planning +//mpPlannerQueue_t mb; // buffer pool management +mpPlanner_t *mp; // currently active planner (global variable) +mpPlanner_t mp0; // primary planning context +mpPlanner_t mp1; // secondary planning context +mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers +mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers //mpMotionPlannerHead_t *mp; // pointer to motion planner //mpMotionPlannerHead_t mp0; // primary motion planner @@ -142,20 +144,71 @@ static stat_t _exec_json_wait(mpBuf_t *bf); /* * planner_init() * planner_reset() + * runtime_init() */ -void planner_init() + +// initialize a planner queue +void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size) +{ + mpBuf_t *pv, *nx; + uint8_t i, nx_i; +// mpQueue_t *b = &mb.q[q]; + mpPlannerQueue_t *q = &(mpl->q); + + memset(q, 0, sizeof(mpPlannerQueue_t)); // clear values, pointers and status + 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; + q->queue_size = size-1; + q->buffers_available = size; + + pv = &q->bf[size-1]; + for (i=0; i < size; i++) { + q->bf[i].buffer_number = i; // number is for diagnostics only (otherwise not used) + nx_i = ((ibf[nx_i]; + q->bf[i].nx = nx; // setup circular list pointers + q->bf[i].pv = pv; + pv = &q->bf[i]; + } + q->bf[size-1].nx = pool; +} + +void planner_init(mpPlanner_t *mpl) { // If you know all memory has been zeroed by a hard reset you don't need these next 2 lines memset(&mp, 0, sizeof(mp)); // clear all values, pointers and status - memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status - planner_init_assertions(); - mp_init_buffers(); - mp.mfo_factor = 1.00; +// memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status + + mp = &mp0; // install the primary planner + _init_planner_queue(mp, mp0_pool, PLANNER_BUFFER_POOL_SIZE); + + planner_init_assertions(mpl); + + mpl->mfo_factor = 1.00; + +// 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() +void planner_reset(mpPlanner_t *mpl) { - planner_init(); + planner_init(mpl); +} + +void runtime_init() { + memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status + + 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]; } /* @@ -163,21 +216,19 @@ void planner_reset() * planner_test_assertions() - test assertions, PANIC if violation exists */ -void planner_init_assertions() +void planner_init_assertions(mpPlanner_t *mpl) { // Note: mb magic numbers set up by mp_init_buffers() - mp.magic_start = MAGICNUM; - mp.magic_end = MAGICNUM; + mpl->magic_start = MAGICNUM; + mpl->magic_end = MAGICNUM; mr.magic_start = MAGICNUM; mr.magic_end = MAGICNUM; } -stat_t planner_test_assertions() +stat_t planner_test_assertions(mpPlanner_t *mpl) { if ( -// (BAD_MAGIC(mb.q[0].magic_start)) || (BAD_MAGIC(mb.q[0].magic_end)) || // kind of a hack -// (BAD_MAGIC(mb.q[1].magic_start)) || (BAD_MAGIC(mb.q[1].magic_end)) || // this one, too - (BAD_MAGIC(mp.magic_start)) || (BAD_MAGIC(mp.magic_end)) || + (BAD_MAGIC(mpl->magic_start)) || (BAD_MAGIC(mpl->magic_end)) || (BAD_MAGIC(mr.magic_start)) || (BAD_MAGIC(mr.magic_end)) ) { return(cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()")); @@ -199,7 +250,7 @@ stat_t planner_test_assertions() void mp_halt_runtime() { stepper_reset(); // stop the steppers and dwells - planner_reset(); // reset the planner queues + planner_reset(mp); // reset the active planner } /* @@ -210,10 +261,11 @@ void mp_halt_runtime() * This function is designed to be called during a hold to reset the planner * This function should not generally be called; call cm_queue_flush() instead */ -void mp_flush_planner() +void mp_flush_planner(mpPlanner_t *mpl) { cm_abort_arc(); - mp_init_buffers(); +// mp_init_planner_buffers(mpl); //+++++ + planner_init(mpl); mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion } @@ -242,7 +294,7 @@ void mp_flush_planner() * still close to the starting point. */ -void mp_set_planner_position(uint8_t axis, const float position) { mp.position[axis] = position; } +void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } //+++++ void mp_set_runtime_position(uint8_t axis, const float position) { mr.position[axis] = position; } void mp_set_steps_to_runtime_position() @@ -450,6 +502,7 @@ 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; }; @@ -468,13 +521,29 @@ 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 *mpl) // which planner are you interested in? +{ + return (mpl->q.buffers_available); +} + +bool mp_planner_is_full(mpPlanner_t *mpl) // which planner are you interested in? +{ + // We also need to ensure we have room for another JSON command + return ((mpl->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); +} + +bool mp_has_runnable_buffer(mpPlanner_t *mpl) // which planner are you interested in?) +{ + return (mpl->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true +} bool mp_is_phat_city_time() { if(cm->hold_state == FEEDHOLD_HOLD) { return true; } - return ((mp.plannable_time <= 0.0) || (PHAT_CITY_TIME < mp.plannable_time)); + return ((mp->plannable_time <= 0.0) || (PHAT_CITY_TIME < mp->plannable_time)); } /* @@ -514,32 +583,32 @@ 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(ACTIVE_Q) == PLANNER_BUFFER_POOL_SIZE) && // detect and set IDLE state + if ((mp_get_planner_buffers(mp) == PLANNER_BUFFER_POOL_SIZE) && //+++++ // detect and set IDLE state (cm->motion_state == MOTION_STOP) && (cm->hold_state == FEEDHOLD_OFF)) { - mp.planner_state = PLANNER_IDLE; + mp->planner_state = PLANNER_IDLE; return (STAT_OK); } - bool _timed_out = mp.block_timeout.isPast(); + bool _timed_out = mp->block_timeout.isPast(); if (_timed_out) { - mp.block_timeout.clear(); // timer is set on commit_write_buffer() + mp->block_timeout.clear(); // timer is set on commit_write_buffer() } - if (!mp.request_planning && !_timed_out) { // Exit if no request or timeout + if (!mp->request_planning && !_timed_out) { // Exit if no request or timeout return (STAT_OK); } // Process a planner request or timeout - if (mp.planner_state == PLANNER_IDLE) { - mp.p = mp_get_r(ACTIVE_Q); // initialize planner pointer to run buffer - mp.planner_state = PLANNER_STARTUP; + if (mp->planner_state == PLANNER_IDLE) { + 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(ACTIVE_Q) && !_timed_out) { + if (mp->planner_state == PLANNER_STARTUP) { + if (!mp_planner_is_full(mp) && !_timed_out) { //+++++ return (STAT_OK); // remain in STARTUP } - mp.planner_state = PLANNER_PRIMING; + mp->planner_state = PLANNER_PRIMING; } mp_plan_block_list(); return (STAT_OK); @@ -559,9 +628,9 @@ 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(ACTIVE_Q)); + } while ((bf = mp_get_next_buffer(bf)) != mp_get_r()); //+++++ - mp.request_planning = true; + mp->request_planning = true; } /* @@ -587,21 +656,21 @@ void mp_start_feed_override(const float ramp_time, const float override_factor) { cm->mfo_state = MFO_REQUESTED; - if (mp.planner_state == PLANNER_IDLE) { - mp.mfo_factor = override_factor; // that was easy + if (mp->planner_state == PLANNER_IDLE) { + mp->mfo_factor = override_factor; // that was easy return; } // Assume that the min and max values for override_factor have been validated upstream // SUVAT: V = U+AT ==> A = (V-U)/T - mp.ramp_target = override_factor; - mp.ramp_dvdt = (override_factor - mp.c->override_factor) / ramp_time; - mp.mfo_active = true; + mp->ramp_target = override_factor; + mp->ramp_dvdt = (override_factor - mp->c->override_factor) / ramp_time; + mp->mfo_active = true; - if (fp_NOT_ZERO(mp.ramp_dvdt)) { // do these things only if you actually have a ramp to run - mp.p = mp.c; // re-position the planner pointer - mp.ramp_active = true; - mp.request_planning = true; + if (fp_NOT_ZERO(mp->ramp_dvdt)) { // do these things only if you actually have a ramp to run + mp->p = mp->c; // re-position the planner pointer + mp->ramp_active = true; + mp->request_planning = true; } } @@ -616,18 +685,18 @@ void mp_end_feed_override(const float ramp_time) void mp_planner_time_accounting() { - mpBuf_t *bf = mp_get_r(ACTIVE_Q); // 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(ACTIVE_Q)) { + mp->plannable_time = 0; //UPDATE_BF_MS(bf); //+++++ + while ((bf = bf->nx) != mp_get_r()) { //+++++ if (bf->buffer_state == MP_BUFFER_EMPTY || bf->plannable == true) { break; } - mp.plannable_time += bf->block_time; + mp->plannable_time += bf->block_time; } UPDATE_MP_DIAGNOSTICS //+++++ } @@ -714,46 +783,18 @@ static inline void _clear_buffer(mpBuf_t *bf) bf->reset(); // Call a reset method on the buffer object. } // We'll need something else for C - like bring the method code back into this function. -// initialize a single planner queue -void _init_planner_queue(uint8_t q, mpBuf_t *pool, uint8_t size) +/* +void mp_init_planner_buffers(void) { - mpBuf_t *pv, *nx; - uint8_t i, nx_i; - mpQueue_t *b = &mb.q[q]; +// _init_planner_queue(0, mb_pool0, PLANNER_BUFFER_POOL_SIZE); +// _init_planner_queue(1, mb_pool1, SECONDARY_BUFFER_POOL_SIZE); - memset(b, 0, sizeof(mpQueue_t)); // clear values, pointers and status - b->magic_start = MAGICNUM; - b->magic_end = MAGICNUM; - - memset(pool, 0, sizeof(mpBuf_t)*size); // clear all buffers in pool - b->bf = pool; // link the buffer pool first - b->w = pool; // init all buffer pointers - b->r = pool; - b->queue_size = size-1; - b->buffers_available = size; - - pv = &b->bf[size-1]; - for (i=0; i < size; i++) { - b->bf[i].buffer_number = i; // number is for diagnostics only (otherwise not used) - nx_i = ((ibf[nx_i]; - b->bf[i].nx = nx; // setup circular list pointers - b->bf[i].pv = pv; - pv = &b->bf[i]; - } - b->bf[size-1].nx = pool; -} - -void mp_init_buffers(void) -{ - _init_planner_queue(0, mb_pool0, PLANNER_BUFFER_POOL_SIZE); - _init_planner_queue(1, mb_pool1, SECONDARY_BUFFER_POOL_SIZE); - - mr.bf[0].nx = &mr.bf[1]; // Now 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]; +// mr.bf[0].nx = &mr.bf[1]; // Now 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]; } +*/ /* * These GET functions are defined here but we use the macros in planner.h instead @@ -761,26 +802,22 @@ 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(int8_t q) { return ((q == ACTIVE_Q) ? mb.q[mb.active_q].w : mb.q[q].w); } -mpBuf_t * mp_get_r(int8_t q) { return ((q == ACTIVE_Q) ? mb.q[mb.active_q].r : mb.q[q].r); } +//mpBuf_t * mp_get_w(int8_t q) { return ((q == ACTIVE_Q) ? mb.q[mb.active_q].w : mb.q[q].w); } +//mpBuf_t * mp_get_r(int8_t q) { return ((q == ACTIVE_Q) ? mb.q[mb.active_q].r : mb.q[q].r); } + +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_write_buffer() // get & clear a buffer { - mpQueue_t *q = &mb.q[mb.active_q]; +// mpQueue_t *q = &mb.q[mb.active_q]; + 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 q->w->buffer_state = MP_BUFFER_INITIALIZING; q->buffers_available--; - return (mp_get_w(ACTIVE_Q)); -/*======= - if (mb.w->buffer_state == MP_BUFFER_EMPTY) { - _clear_buffer(mb.w); // ++++RG this is redundant, it was just cleared in mp_free_run_buffer - mb.w->buffer_state = MP_BUFFER_INITIALIZING; - mb.buffers_available--; - return (mb.w); ->>>>>>> refs/heads/edge -*/ + return (mp_get_w()); } // The no buffer condition always causes a panic - invoked by the caller rpt_exception(STAT_FAILED_TO_GET_PLANNER_BUFFER, "mp_get_write_buffer()"); @@ -789,7 +826,8 @@ mpBuf_t * mp_get_write_buffer() // get & clear a buffer void mp_unget_write_buffer() // mark buffer as empty and adjust free buffer count { - mpQueue_t *q = &mb.q[mb.active_q]; +// mpQueue_t *q = &mb.q[mb.active_q]; + mpPlannerQueue_t *q = &(mp->q); if (q->w->buffer_state != MP_BUFFER_EMPTY) { // safety. Can't unget an empty buffer q->w->buffer_state = MP_BUFFER_EMPTY; @@ -804,7 +842,8 @@ void mp_unget_write_buffer() // mark buffer as empty and adjust free buff void mp_commit_write_buffer(const blockType block_type) { - mpQueue_t *q = &mb.q[mb.active_q]; +// mpQueue_t *q = &mb.q[mb.active_q]; + mpPlannerQueue_t *q = &(mp->q); q->w->block_type = block_type; q->w->block_state = BLOCK_INITIAL_ACTION; @@ -814,16 +853,16 @@ void mp_commit_write_buffer(const blockType block_type) cm_set_motion_state(MOTION_PLANNING); } } else { - if ((mp.planner_state > PLANNER_STARTUP) && (cm->hold_state == FEEDHOLD_OFF)) { + if ((mp->planner_state > PLANNER_STARTUP) && (cm->hold_state == FEEDHOLD_OFF)) { // NB: BEWARE! the requested exec may result in the planner buffer being // processed IMMEDIATELY and then freed - invalidating the contents st_request_forward_plan(); // request an exec if the runtime is not busy } } q->w->plannable = true; // enable block for planning - mp.request_planning = true; + mp->request_planning = true; q->w = q->w->nx; // advance write buffer pointer - mp.block_timeout.set(BLOCK_TIMEOUT_MS); // reset the block timer + mp->block_timeout.set(BLOCK_TIMEOUT_MS); // reset the block timer qr_request_queue_report(+1); // request QR and add to "added buffers" count } @@ -834,7 +873,8 @@ void mp_commit_write_buffer(const blockType block_type) // (2) is the buffer in error - i.e. not yet ready for running? mpBuf_t * mp_get_run_buffer() { - mpBuf_t *r = mb.q[mb.active_q].r; +// mpBuf_t *r = mb.q[mb.active_q].r; + mpBuf_t *r = mp->q.r; if (r->buffer_state == MP_BUFFER_EMPTY) { return (NULL); @@ -846,7 +886,9 @@ mpBuf_t * mp_get_run_buffer() // Clearing and advancing must be done atomically as other interrupts may be using the run buffer bool mp_free_run_buffer() // EMPTY current run buffer & advance to the next { - mpQueue_t *q = &mb.q[mb.active_q]; +// mpQueue_t *q = &mb.q[mb.active_q]; + mpPlannerQueue_t *q = &(mp->q); + 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) diff --git a/g2core/planner.h b/g2core/planner.h index d18db589..4c15ef8a 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -241,12 +241,13 @@ typedef enum { ZOID_EXIT_3a2 } zoidExitPoint; +/* +++++ REMOVE typedef enum { ACTIVE_Q = -1, // Must be -1 PRIMARY_Q, // must line up with structure indexes SECONDARY_Q } queueType; - +*/ /*** 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 @@ -298,7 +299,7 @@ typedef enum { //#define ASCII_ART(s) xio_writeline(s) #define ASCII_ART(s) //#define UPDATE_BF_DIAGNOSTICS(bf) { bf->block_time_ms = bf->block_time*60000; bf->plannable_time_ms = bf->plannable_time*60000; } -#define UPDATE_MP_DIAGNOSTICS { mp.plannable_time_ms = mp.plannable_time*60000; } +#define UPDATE_MP_DIAGNOSTICS { mp->plannable_time_ms = mp->plannable_time*60000; } /* * Planner structures @@ -411,7 +412,7 @@ typedef struct mpBuffer { } mpBuf_t; -typedef struct mpQueue { // a planner buffer queue +typedef struct mpPlannerQueue { // control structure for queue magic_t magic_start; // magic number to test memory integrity mpBuf_t *r; // run buffer pointer mpBuf_t *w; // write buffer pointer @@ -419,15 +420,17 @@ typedef struct mpQueue { // a planner buffer queue uint8_t buffers_available; // running count of available buffers in queue mpBuf_t *bf; // pointer to buffer storage array magic_t magic_end; -} mpQueue_t; +} mpPlannerQueue_t; +/* typedef struct mpBufferQueue { // one or more planner buffer queues uint8_t active_q; // index of currently active queue uint8_t return_q; // index of queue to return to mpQueue_t q[2]; // number of queues } mpBufferQueue_t; +*/ -typedef struct mpMotionPlannerHead {// common variables for planning (move master) +typedef struct mpPlanner { // common variables for a planner context magic_t magic_start; // magic number to test memory integrity //+++++ DIAGNOSTICS @@ -463,10 +466,10 @@ typedef struct mpMotionPlannerHead {// common variables for planning (move maste mpBuf_t *planning_return; // buffer to return to once back-planning is complete // queue manager - mpBufferQueue_t q; // embed a buffer queue manager + mpPlannerQueue_t q; // embed a planner buffer queue manager magic_t magic_end; -} mpMotionPlannerHead_t; +} mpPlanner_t; typedef struct mpBlockRuntimeBuf { // Data structure for just the parts of RunTime that we need to plan a BLOCK struct mpBlockRuntimeBuf *nx; // singly-linked-list @@ -525,8 +528,14 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables } mpMotionRuntimeSingleton_t; // Reference global scope structures -extern mpBufferQueue_t mb; // planner buffer queue management -extern mpMotionPlannerHead_t mp; // context for block planning +//extern mpPlannerQueue_t mb; // planner buffer queue management + +extern mpPlanner_t *mp; // currently active planner (global variable) +extern mpPlanner_t mp0; // primary planning context +extern mpPlanner_t mp1; // secondary planning context +extern mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers +extern mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers + extern mpMotionRuntimeSingleton_t mr; // context for block runtime /* @@ -535,13 +544,14 @@ extern mpMotionRuntimeSingleton_t mr; // context for block runtime //planner.cpp functions -void planner_init(void); -void planner_reset(void); -void planner_init_assertions(void); -stat_t planner_test_assertions(void); +void planner_init(mpPlanner_t *mpl); +void planner_reset(mpPlanner_t *mpl); +void planner_init_assertions(mpPlanner_t *mpl); +stat_t planner_test_assertions(mpPlanner_t *mpl); +void runtime_init(void); void mp_halt_runtime(void); -void mp_flush_planner(void); +void mp_flush_planner(mpPlanner_t *mpl); void mp_set_planner_position(uint8_t axis, const float position); void mp_set_runtime_position(uint8_t axis, const float position); void mp_set_steps_to_runtime_position(void); @@ -558,9 +568,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(int8_t q); -bool mp_planner_is_full(int8_t q); -bool mp_has_runnable_buffer(int8_t q); +uint8_t mp_get_planner_buffers(mpPlanner_t *mpl); +bool mp_planner_is_full(mpPlanner_t *mpl); +bool mp_has_runnable_buffer(mpPlanner_t *mpl); bool mp_is_phat_city_time(void); stat_t mp_planner_callback(); @@ -570,9 +580,11 @@ void mp_end_feed_override(const float ramp_time); void mp_planner_time_accounting(void); // planner buffer primitives -void mp_init_buffers(void); -mpBuf_t * mp_get_w(int8_t q); -mpBuf_t * mp_get_r(int8_t q); +//void mp_init_planner_buffers(void); +//mpBuf_t * mp_get_w(int8_t q); +//mpBuf_t * mp_get_r(int8_t q); +mpBuf_t * mp_get_w(void); +mpBuf_t * mp_get_r(void); //mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf); // Use the following macro instead //mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf); // Use the following macro instead diff --git a/g2core/report.cpp b/g2core/report.cpp index ad1d3da6..27cdfb1d 100644 --- a/g2core/report.cpp +++ b/g2core/report.cpp @@ -483,7 +483,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(ACTIVE_Q); + qr.buffers_available = mp_get_planner_buffers(mp); //+++++ if (buffers > 0) { qr.buffers_added += buffers; } else { @@ -567,7 +567,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(ACTIVE_Q); // 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); }