diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 253ff5e1..5f25a020 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -591,10 +591,8 @@ stat_t cm_set_tram(nvObj_t *nv) // Step 1b: compute the combined magnitude // since sqrt(a)*sqrt(b) = sqrt(a*b), we can save a sqrt in making the unit normal - float combined_magnitude_inv = 1.0/sqrt( - (d0_x*d0_x + d0_y*d0_y + d0_z*d0_z)* - (d2_x*d2_x + d2_y*d2_y + d2_z*d2_z) - ); + float combined_magnitude_inv = 1.0/sqrt((d0_x*d0_x + d0_y*d0_y + d0_z*d0_z) * + (d2_x*d2_x + d2_y*d2_y + d2_z*d2_z)); // Step 1c: compute the cross product and normalize float n_x = (d0_z*d2_y - d0_y*d2_z)*combined_magnitude_inv; @@ -894,6 +892,7 @@ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, else { return (STAT_L_WORD_IS_INVALID); } + cm_set_display_offsets(MODEL); return (STAT_OK); } @@ -1071,6 +1070,7 @@ stat_t cm_set_origin_offsets(const float offset[], const bool flag[]) // now pass the offset to the callback - setting the coordinate system also applies the offsets float value[] = { (float)cm->gm.coord_system }; // pass coordinate system in value[0] element mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } @@ -1082,6 +1082,7 @@ stat_t cm_reset_origin_offsets() } float value[] = { (float)cm->gm.coord_system }; mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } @@ -1090,6 +1091,7 @@ stat_t cm_suspend_origin_offsets() cm->gmx.origin_offset_enable = false; float value[] = { (float)cm->gm.coord_system }; mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } @@ -1098,6 +1100,7 @@ stat_t cm_resume_origin_offsets() cm->gmx.origin_offset_enable = true; float value[] = { (float)cm->gm.coord_system }; mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 80445f63..9c2cc5b4 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -756,7 +756,8 @@ static stat_t _exec_aline_head(mpBuf_t *bf) if (mr->segment_time < MIN_SEGMENT_TIME) { debug_trap("mr->segment_time < MIN_SEGMENT_TIME (head)"); return (STAT_OK); // exit without advancing position, say we're done - } + } + debug_trap_if_true(mr->section != SECTION_HEAD, "exec_aline() Not section head"); mr->section = SECTION_HEAD; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; } else { @@ -801,6 +802,7 @@ static stat_t _exec_aline_body(mpBuf_t *bf) debug_trap("mr->segment_time < MIN_SEGMENT_TIME (body)"); return (STAT_OK); // exit without advancing position, say we're done } + debug_trap_if_true(mr->section != SECTION_BODY, "exec_aline() Not section body"); mr->section = SECTION_BODY; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; // uses PERIOD_2 so last segment detection works } @@ -841,6 +843,7 @@ static stat_t _exec_aline_tail(mpBuf_t *bf) debug_trap("mr->segment_time < MIN_SEGMENT_TIME (tail)"); return (STAT_OK); // exit without advancing position, say we're done } + debug_trap_if_true(mr->section != SECTION_TAIL, "exec_aline() Not section tail"); mr->section = SECTION_TAIL; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; } else {