Clean up "else" and other spacing
This commit is contained in:
parent
9b23490f01
commit
9bdab4f3a8
|
@ -78,7 +78,8 @@ void MarlinSerial::begin(long baud) {
|
||||||
if (useU2X) {
|
if (useU2X) {
|
||||||
M_UCSRxA = BIT(M_U2Xx);
|
M_UCSRxA = BIT(M_U2Xx);
|
||||||
baud_setting = (F_CPU / 4 / baud - 1) / 2;
|
baud_setting = (F_CPU / 4 / baud - 1) / 2;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
M_UCSRxA = 0;
|
M_UCSRxA = 0;
|
||||||
baud_setting = (F_CPU / 8 / baud - 1) / 2;
|
baud_setting = (F_CPU / 8 / baud - 1) / 2;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1688,7 +1688,8 @@ static void setup_for_endstop_move() {
|
||||||
if (a < b) {
|
if (a < b) {
|
||||||
if (b < c) median = b;
|
if (b < c) median = b;
|
||||||
if (c < a) median = a;
|
if (c < a) median = a;
|
||||||
} else { // b <= a
|
}
|
||||||
|
else { // b <= a
|
||||||
if (c < b) median = b;
|
if (c < b) median = b;
|
||||||
if (a < c) median = a;
|
if (a < c) median = a;
|
||||||
}
|
}
|
||||||
|
@ -1783,7 +1784,8 @@ static void setup_for_endstop_move() {
|
||||||
#endif
|
#endif
|
||||||
do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1); // Dock sled a bit closer to ensure proper capturing
|
do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1); // Dock sled a bit closer to ensure proper capturing
|
||||||
digitalWrite(SLED_PIN, LOW); // turn off magnet
|
digitalWrite(SLED_PIN, LOW); // turn off magnet
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
float z_loc = current_position[Z_AXIS];
|
float z_loc = current_position[Z_AXIS];
|
||||||
if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
|
if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
|
||||||
do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], z_loc); // this also updates current_position
|
do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], z_loc); // this also updates current_position
|
||||||
|
@ -2696,7 +2698,8 @@ inline void gcode_G28() {
|
||||||
SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
|
SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
SERIAL_PROTOCOLPGM("X not entered.\n");
|
SERIAL_PROTOCOLPGM("X not entered.\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -2706,7 +2709,8 @@ inline void gcode_G28() {
|
||||||
SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
|
SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
SERIAL_PROTOCOLPGM("Y not entered.\n");
|
SERIAL_PROTOCOLPGM("Y not entered.\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -6381,25 +6385,29 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
|
||||||
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
||||||
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
||||||
x_splits ^= BIT(ix);
|
x_splits ^= BIT(ix);
|
||||||
} else if (ix < pix && (x_splits) & BIT(pix)) {
|
}
|
||||||
|
else if (ix < pix && (x_splits) & BIT(pix)) {
|
||||||
nx = mbl.get_x(pix);
|
nx = mbl.get_x(pix);
|
||||||
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
|
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
|
||||||
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
||||||
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
||||||
x_splits ^= BIT(pix);
|
x_splits ^= BIT(pix);
|
||||||
} else if (iy > piy && (y_splits) & BIT(iy)) {
|
}
|
||||||
|
else if (iy > piy && (y_splits) & BIT(iy)) {
|
||||||
ny = mbl.get_y(iy);
|
ny = mbl.get_y(iy);
|
||||||
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
|
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
|
||||||
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
||||||
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
||||||
y_splits ^= BIT(iy);
|
y_splits ^= BIT(iy);
|
||||||
} else if (iy < piy && (y_splits) & BIT(piy)) {
|
}
|
||||||
|
else if (iy < piy && (y_splits) & BIT(piy)) {
|
||||||
ny = mbl.get_y(piy);
|
ny = mbl.get_y(piy);
|
||||||
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
|
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
|
||||||
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
||||||
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
||||||
y_splits ^= BIT(piy);
|
y_splits ^= BIT(piy);
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
// Already split on a border
|
// Already split on a border
|
||||||
plan_buffer_line(x, y, z, e, feed_rate, extruder);
|
plan_buffer_line(x, y, z, e, feed_rate, extruder);
|
||||||
set_current_to_destination();
|
set_current_to_destination();
|
||||||
|
|
|
@ -192,11 +192,13 @@ uint32_t Sd2Card::cardSize() {
|
||||||
uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1)
|
uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1)
|
||||||
| csd.v1.c_size_mult_low;
|
| csd.v1.c_size_mult_low;
|
||||||
return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7);
|
return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7);
|
||||||
} else if (csd.v2.csd_ver == 1) {
|
}
|
||||||
|
else if (csd.v2.csd_ver == 1) {
|
||||||
uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16)
|
uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16)
|
||||||
| (csd.v2.c_size_mid << 8) | csd.v2.c_size_low;
|
| (csd.v2.c_size_mid << 8) | csd.v2.c_size_low;
|
||||||
return (c_size + 1) << 10;
|
return (c_size + 1) << 10;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
error(SD_CARD_ERROR_BAD_CSD);
|
error(SD_CARD_ERROR_BAD_CSD);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -396,7 +396,8 @@ static inline __attribute__((always_inline))
|
||||||
bool getPinMode(uint8_t pin) {
|
bool getPinMode(uint8_t pin) {
|
||||||
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
||||||
return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1;
|
return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
return badPinNumber();
|
return badPinNumber();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -405,10 +406,12 @@ static inline __attribute__((always_inline))
|
||||||
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
||||||
if (mode) {
|
if (mode) {
|
||||||
*digitalPinMap[pin].ddr |= BIT(digitalPinMap[pin].bit);
|
*digitalPinMap[pin].ddr |= BIT(digitalPinMap[pin].bit);
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
*digitalPinMap[pin].ddr &= ~BIT(digitalPinMap[pin].bit);
|
*digitalPinMap[pin].ddr &= ~BIT(digitalPinMap[pin].bit);
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
badPinNumber();
|
badPinNumber();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -416,7 +419,8 @@ static inline __attribute__((always_inline))
|
||||||
bool fastDigitalRead(uint8_t pin) {
|
bool fastDigitalRead(uint8_t pin) {
|
||||||
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
||||||
return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1;
|
return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
return badPinNumber();
|
return badPinNumber();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -425,10 +429,12 @@ static inline __attribute__((always_inline))
|
||||||
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
if (__builtin_constant_p(pin) && pin < digitalPinCount) {
|
||||||
if (value) {
|
if (value) {
|
||||||
*digitalPinMap[pin].port |= BIT(digitalPinMap[pin].bit);
|
*digitalPinMap[pin].port |= BIT(digitalPinMap[pin].bit);
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
*digitalPinMap[pin].port &= ~BIT(digitalPinMap[pin].bit);
|
*digitalPinMap[pin].port &= ~BIT(digitalPinMap[pin].bit);
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
badPinNumber();
|
badPinNumber();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -206,9 +206,11 @@ static void lcd_implementation_init() {
|
||||||
pinMode(LCD_PIN_RESET, OUTPUT);
|
pinMode(LCD_PIN_RESET, OUTPUT);
|
||||||
digitalWrite(LCD_PIN_RESET, HIGH);
|
digitalWrite(LCD_PIN_RESET, HIGH);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if DISABLED(MINIPANEL) // setContrast not working for Mini Panel
|
#if DISABLED(MINIPANEL) // setContrast not working for Mini Panel
|
||||||
u8g.setContrast(lcd_contrast);
|
u8g.setContrast(lcd_contrast);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// FIXME: remove this workaround
|
// FIXME: remove this workaround
|
||||||
// Uncomment this if you have the first generation (V1.10) of STBs board
|
// Uncomment this if you have the first generation (V1.10) of STBs board
|
||||||
// pinMode(17, OUTPUT); // Enable LCD backlight
|
// pinMode(17, OUTPUT); // Enable LCD backlight
|
||||||
|
|
|
@ -30,15 +30,15 @@
|
||||||
|
|
||||||
#define _WRITE_C(IO, v) do { if (v) { \
|
#define _WRITE_C(IO, v) do { if (v) { \
|
||||||
CRITICAL_SECTION_START; \
|
CRITICAL_SECTION_START; \
|
||||||
{DIO ## IO ## _WPORT |= MASK(DIO ## IO ## _PIN); }\
|
{DIO ## IO ## _WPORT |= MASK(DIO ## IO ## _PIN); } \
|
||||||
CRITICAL_SECTION_END; \
|
CRITICAL_SECTION_END; \
|
||||||
}\
|
} \
|
||||||
else {\
|
else { \
|
||||||
CRITICAL_SECTION_START; \
|
CRITICAL_SECTION_START; \
|
||||||
{DIO ## IO ## _WPORT &= ~MASK(DIO ## IO ## _PIN); }\
|
{DIO ## IO ## _WPORT &= ~MASK(DIO ## IO ## _PIN); } \
|
||||||
CRITICAL_SECTION_END; \
|
CRITICAL_SECTION_END; \
|
||||||
}\
|
} \
|
||||||
}\
|
} \
|
||||||
while (0)
|
while (0)
|
||||||
|
|
||||||
#define _WRITE(IO, v) do { if (&(DIO ## IO ## _RPORT) >= (uint8_t *)0x100) {_WRITE_C(IO, v); } else {_WRITE_NC(IO, v); }; } while (0)
|
#define _WRITE(IO, v) do { if (&(DIO ## IO ## _RPORT) >= (uint8_t *)0x100) {_WRITE_C(IO, v); } else {_WRITE_NC(IO, v); }; } while (0)
|
||||||
|
|
|
@ -439,10 +439,12 @@ void check_axes_activity() {
|
||||||
// Just starting up fan - run at full power.
|
// Just starting up fan - run at full power.
|
||||||
fan_kick_end = ms + FAN_KICKSTART_TIME;
|
fan_kick_end = ms + FAN_KICKSTART_TIME;
|
||||||
tail_fan_speed = 255;
|
tail_fan_speed = 255;
|
||||||
} else if (fan_kick_end > ms)
|
}
|
||||||
|
else if (fan_kick_end > ms)
|
||||||
// Fan still spinning up.
|
// Fan still spinning up.
|
||||||
tail_fan_speed = 255;
|
tail_fan_speed = 255;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
fan_kick_end = 0;
|
fan_kick_end = 0;
|
||||||
}
|
}
|
||||||
#endif //FAN_KICKSTART_TIME
|
#endif //FAN_KICKSTART_TIME
|
||||||
|
|
|
@ -494,7 +494,8 @@ double dnrm2(int n, double x[], int incx)
|
||||||
if (scale < absxi) {
|
if (scale < absxi) {
|
||||||
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
|
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
|
||||||
scale = absxi;
|
scale = absxi;
|
||||||
} else
|
}
|
||||||
|
else
|
||||||
ssq = ssq + (absxi / scale) * (absxi / scale);
|
ssq = ssq + (absxi / scale) * (absxi / scale);
|
||||||
}
|
}
|
||||||
ix += incx;
|
ix += incx;
|
||||||
|
@ -1404,7 +1405,8 @@ void dscal(int n, double sa, double x[], int incx)
|
||||||
x[i + 3] = sa * x[i + 3];
|
x[i + 3] = sa * x[i + 3];
|
||||||
x[i + 4] = sa * x[i + 4];
|
x[i + 4] = sa * x[i + 4];
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
if (0 <= incx)
|
if (0 <= incx)
|
||||||
ix = 0;
|
ix = 0;
|
||||||
else
|
else
|
||||||
|
@ -1486,7 +1488,6 @@ void dswap(int n, double x[], int incx, double y[], int incy)
|
||||||
x[i + 2] = y[i + 2];
|
x[i + 2] = y[i + 2];
|
||||||
y[i + 2] = temp;
|
y[i + 2] = temp;
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
if (0 <= incx)
|
if (0 <= incx)
|
||||||
ix = 0;
|
ix = 0;
|
||||||
else
|
else
|
||||||
|
@ -1495,6 +1496,8 @@ void dswap(int n, double x[], int incx, double y[], int incy)
|
||||||
iy = 0;
|
iy = 0;
|
||||||
else
|
else
|
||||||
iy = (- n + 1) * incy;
|
iy = (- n + 1) * incy;
|
||||||
|
}
|
||||||
|
else {
|
||||||
for (i = 0; i < n; i++) {
|
for (i = 0; i < n; i++) {
|
||||||
temp = x[ix];
|
temp = x[ix];
|
||||||
x[ix] = y[iy];
|
x[ix] = y[iy];
|
||||||
|
|
|
@ -139,11 +139,13 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
|
||||||
if (Z_HOME_DIR > 0) {\
|
if (Z_HOME_DIR > 0) {\
|
||||||
if (!(TEST(old_endstop_bits, Z_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
|
if (!(TEST(old_endstop_bits, Z_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
|
||||||
if (!(TEST(old_endstop_bits, Z2_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
|
if (!(TEST(old_endstop_bits, Z2_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
|
||||||
} else {\
|
} \
|
||||||
|
else { \
|
||||||
if (!(TEST(old_endstop_bits, Z_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
|
if (!(TEST(old_endstop_bits, Z_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
|
||||||
if (!(TEST(old_endstop_bits, Z2_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
|
if (!(TEST(old_endstop_bits, Z2_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
|
||||||
} \
|
} \
|
||||||
} else { \
|
} \
|
||||||
|
else { \
|
||||||
Z_STEP_WRITE(v); \
|
Z_STEP_WRITE(v); \
|
||||||
Z2_STEP_WRITE(v); \
|
Z2_STEP_WRITE(v); \
|
||||||
}
|
}
|
||||||
|
|
|
@ -511,7 +511,8 @@ float get_pid_output(int e) {
|
||||||
if (e_position > last_position[e]) {
|
if (e_position > last_position[e]) {
|
||||||
lpq[lpq_ptr++] = e_position - last_position[e];
|
lpq[lpq_ptr++] = e_position - last_position[e];
|
||||||
last_position[e] = e_position;
|
last_position[e] = e_position;
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
lpq[lpq_ptr++] = 0;
|
lpq[lpq_ptr++] = 0;
|
||||||
}
|
}
|
||||||
if (lpq_ptr >= lpq_len) lpq_ptr = 0;
|
if (lpq_ptr >= lpq_len) lpq_ptr = 0;
|
||||||
|
|
|
@ -123,10 +123,12 @@
|
||||||
#endif // SIMULATE_ROMFONT
|
#endif // SIMULATE_ROMFONT
|
||||||
|
|
||||||
#if ENABLED(MAPPER_NON)
|
#if ENABLED(MAPPER_NON)
|
||||||
char charset_mapper(char c){
|
|
||||||
|
char charset_mapper(char c) {
|
||||||
HARDWARE_CHAR_OUT( c );
|
HARDWARE_CHAR_OUT( c );
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif ENABLED(MAPPER_C2C3)
|
#elif ENABLED(MAPPER_C2C3)
|
||||||
uint8_t utf_hi_char; // UTF-8 high part
|
uint8_t utf_hi_char; // UTF-8 high part
|
||||||
bool seen_c2 = false;
|
bool seen_c2 = false;
|
||||||
|
@ -138,12 +140,12 @@
|
||||||
seen_c2 = true;
|
seen_c2 = true;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
else if (seen_c2){
|
else if (seen_c2) {
|
||||||
d &= 0x3f;
|
d &= 0x3f;
|
||||||
#ifndef MAPPER_ONE_TO_ONE
|
#ifndef MAPPER_ONE_TO_ONE
|
||||||
HARDWARE_CHAR_OUT( (char) pgm_read_byte_near( utf_recode + d + ( utf_hi_char << 6 ) - 0x20 ) );
|
HARDWARE_CHAR_OUT((char)pgm_read_byte_near(utf_recode + d + (utf_hi_char << 6) - 0x20));
|
||||||
#else
|
#else
|
||||||
HARDWARE_CHAR_OUT( (char) (0x80 + ( utf_hi_char << 6 ) + d) ) ;
|
HARDWARE_CHAR_OUT((char)(0x80 + (utf_hi_char << 6) + d)) ;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
@ -156,96 +158,114 @@
|
||||||
seen_c2 = false;
|
seen_c2 = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif ENABLED(MAPPER_D0D1_MOD)
|
#elif ENABLED(MAPPER_D0D1_MOD)
|
||||||
uint8_t utf_hi_char; // UTF-8 high part
|
uint8_t utf_hi_char; // UTF-8 high part
|
||||||
bool seen_d5 = false;
|
bool seen_d5 = false;
|
||||||
char charset_mapper(char c){
|
|
||||||
|
char charset_mapper(char c) {
|
||||||
// it is a Russian alphabet translation
|
// it is a Russian alphabet translation
|
||||||
// except 0401 --> 0xa2 = Ё, 0451 --> 0xb5 = ё
|
// except 0401 --> 0xa2 = Ё, 0451 --> 0xb5 = ё
|
||||||
uint8_t d = c;
|
uint8_t d = c;
|
||||||
if ( d >= 0x80 ) { // UTF-8 handling
|
if (d >= 0x80) { // UTF-8 handling
|
||||||
if ((d >= 0xd0) && (!seen_d5)) {
|
if (d >= 0xd0 && !seen_d5) {
|
||||||
utf_hi_char = d - 0xd0;
|
utf_hi_char = d - 0xd0;
|
||||||
seen_d5 = true;
|
seen_d5 = true;
|
||||||
return 0;
|
return 0;
|
||||||
} else if (seen_d5) {
|
}
|
||||||
|
else if (seen_d5) {
|
||||||
d &= 0x3f;
|
d &= 0x3f;
|
||||||
if ( !utf_hi_char && ( d == 1 )) {
|
if (!utf_hi_char && d == 1) {
|
||||||
HARDWARE_CHAR_OUT((char) 0xa2 ); // Ё
|
HARDWARE_CHAR_OUT((char) 0xa2); // Ё
|
||||||
} else if ((utf_hi_char == 1) && (d == 0x11)) {
|
}
|
||||||
HARDWARE_CHAR_OUT((char) 0xb5 ); // ё
|
else if (utf_hi_char == 1 && d == 0x11) {
|
||||||
} else {
|
HARDWARE_CHAR_OUT((char)0xb5); // ё
|
||||||
HARDWARE_CHAR_OUT((char) pgm_read_byte_near( utf_recode + d + ( utf_hi_char << 6 ) - 0x10 ) );
|
}
|
||||||
|
else {
|
||||||
|
HARDWARE_CHAR_OUT((char)pgm_read_byte_near(utf_recode + d + (utf_hi_char << 6) - 0x10));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
HARDWARE_CHAR_OUT('?');
|
HARDWARE_CHAR_OUT('?');
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
HARDWARE_CHAR_OUT((char) c );
|
HARDWARE_CHAR_OUT((char) c );
|
||||||
}
|
}
|
||||||
seen_d5 = false;
|
seen_d5 = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif ENABLED(MAPPER_D0D1)
|
#elif ENABLED(MAPPER_D0D1)
|
||||||
uint8_t utf_hi_char; // UTF-8 high part
|
uint8_t utf_hi_char; // UTF-8 high part
|
||||||
bool seen_d5 = false;
|
bool seen_d5 = false;
|
||||||
char charset_mapper(char c) {
|
char charset_mapper(char c) {
|
||||||
uint8_t d = c;
|
uint8_t d = c;
|
||||||
if ( d >= 0x80u ) { // UTF-8 handling
|
if (d >= 0x80u) { // UTF-8 handling
|
||||||
if ((d >= 0xd0u) && (!seen_d5)) {
|
if (d >= 0xd0u && !seen_d5) {
|
||||||
utf_hi_char = d - 0xd0u;
|
utf_hi_char = d - 0xd0u;
|
||||||
seen_d5 = true;
|
seen_d5 = true;
|
||||||
return 0;
|
return 0;
|
||||||
} else if (seen_d5) {
|
}
|
||||||
|
else if (seen_d5) {
|
||||||
d &= 0x3fu;
|
d &= 0x3fu;
|
||||||
#ifndef MAPPER_ONE_TO_ONE
|
#ifndef MAPPER_ONE_TO_ONE
|
||||||
HARDWARE_CHAR_OUT( (char) pgm_read_byte_near( utf_recode + d + ( utf_hi_char << 6 ) - 0x20 ) );
|
HARDWARE_CHAR_OUT((char)pgm_read_byte_near(utf_recode + d + (utf_hi_char << 6) - 0x20));
|
||||||
#else
|
#else
|
||||||
HARDWARE_CHAR_OUT( (char) (0xa0u + ( utf_hi_char << 6 ) + d ) ) ;
|
HARDWARE_CHAR_OUT((char)(0xa0u + (utf_hi_char << 6) + d)) ;
|
||||||
#endif
|
#endif
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
HARDWARE_CHAR_OUT('?');
|
HARDWARE_CHAR_OUT('?');
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
HARDWARE_CHAR_OUT((char) c );
|
HARDWARE_CHAR_OUT((char) c );
|
||||||
}
|
}
|
||||||
seen_d5 = false;
|
seen_d5 = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif ENABLED(MAPPER_E382E383)
|
#elif ENABLED(MAPPER_E382E383)
|
||||||
uint8_t utf_hi_char; // UTF-8 high part
|
uint8_t utf_hi_char; // UTF-8 high part
|
||||||
bool seen_e3 = false;
|
bool seen_e3 = false;
|
||||||
bool seen_82_83 = false;
|
bool seen_82_83 = false;
|
||||||
char charset_mapper(char c){
|
char charset_mapper(char c){
|
||||||
uint8_t d = c;
|
uint8_t d = c;
|
||||||
if ( d >= 0x80 ) { // UTF-8 handling
|
if (d >= 0x80) { // UTF-8 handling
|
||||||
if ( (d == 0xe3) && (seen_e3 == false)) {
|
if (d == 0xe3 && !seen_e3) {
|
||||||
seen_e3 = true;
|
seen_e3 = true;
|
||||||
return 0; // eat 0xe3
|
return 0; // eat 0xe3
|
||||||
} else if ( (d >= 0x82) && (seen_e3 == true) && (seen_82_83 == false)) {
|
}
|
||||||
|
else if (d >= 0x82 && seen_e3 && !seen_82_83) {
|
||||||
utf_hi_char = d - 0x82;
|
utf_hi_char = d - 0x82;
|
||||||
seen_82_83 = true;
|
seen_82_83 = true;
|
||||||
return 0;
|
return 0;
|
||||||
} else if ((seen_e3 == true) && (seen_82_83 == true)){
|
}
|
||||||
|
else if (seen_e3 && seen_82_83) {
|
||||||
d &= 0x3f;
|
d &= 0x3f;
|
||||||
#ifndef MAPPER_ONE_TO_ONE
|
#ifndef MAPPER_ONE_TO_ONE
|
||||||
HARDWARE_CHAR_OUT( (char) pgm_read_byte_near( utf_recode + d + ( utf_hi_char << 6 ) - 0x20 ) );
|
HARDWARE_CHAR_OUT((char)pgm_read_byte_near(utf_recode + d + (utf_hi_char << 6) - 0x20));
|
||||||
#else
|
#else
|
||||||
HARDWARE_CHAR_OUT( (char) (0x80 + ( utf_hi_char << 6 ) + d ) ) ;
|
HARDWARE_CHAR_OUT((char)(0x80 + (utf_hi_char << 6) + d)) ;
|
||||||
#endif
|
#endif
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
HARDWARE_CHAR_OUT((char) '?' );
|
HARDWARE_CHAR_OUT((char) '?' );
|
||||||
}
|
}
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
HARDWARE_CHAR_OUT((char) c );
|
HARDWARE_CHAR_OUT((char) c );
|
||||||
}
|
}
|
||||||
seen_e3 = false;
|
seen_e3 = false;
|
||||||
seen_82_83 = false;
|
seen_82_83 = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#error "You have to define one of the DISPLAY_INPUT_CODE_MAPPERs in your language_xx.h file" // should not occur because (en) will set.
|
#error "You have to define one of the DISPLAY_INPUT_CODE_MAPPERs in your language_xx.h file" // should not occur because (en) will set.
|
||||||
|
|
||||||
#endif // code mappers
|
#endif // code mappers
|
||||||
|
|
||||||
#endif // UTF_MAPPER_H
|
#endif // UTF_MAPPER_H
|
||||||
|
|
Loading…
Reference in a new issue