static void rgb_cs_to_spotn_cm(gx_device * dev, const gs_imager_state *pis, frac r, frac g, frac b, frac out[]) { /* TO_DO_DEVICEN This routine needs to include the effects of the SeparationOrder array */ xcf_device *xdev = (xcf_device *)dev; int n = xdev->separation_names.num_names; icmLuBase *luo = xdev->lu_rgb; int i; if (luo != NULL) { double in[3]; double tmp[MAX_CHAN]; int outn = xdev->lu_rgb_outn; in[0] = frac2float(r); in[1] = frac2float(g); in[2] = frac2float(b); luo->lookup(luo, tmp, in); for (i = 0; i < outn; i++) out[i] = float2frac(tmp[i]); for (; i < n + 4; i++) out[i] = 0; } else { frac cmyk[4]; color_rgb_to_cmyk(r, g, b, pis, cmyk); cmyk_cs_to_spotn_cm(dev, cmyk[0], cmyk[1], cmyk[2], cmyk[3], out); } }
/* currentcmykcolor */ int gs_currentcmykcolor(const gs_state *pgs, float pr4[4]) { const gs_client_color *pcc = pgs->ccolor; switch ( pgs->color_space->type->index ) { case gs_color_space_index_DeviceGray: pr4[0] = pr4[1] = pr4[2] = 0.0; pr4[3] = 1.0 - pcc->paint.values[0]; break; case gs_color_space_index_DeviceRGB: { frac fcmyk[4]; color_rgb_to_cmyk( float2frac(pcc->paint.values[0]), float2frac(pcc->paint.values[1]), float2frac(pcc->paint.values[2]), (const gs_imager_state *)pgs, fcmyk); pr4[0] = frac2float(fcmyk[0]); pr4[1] = frac2float(fcmyk[1]); pr4[2] = frac2float(fcmyk[2]); pr4[3] = frac2float(fcmyk[3]); } break; case gs_color_space_index_DeviceCMYK: pr4[0] = pcc->paint.values[0]; pr4[1] = pcc->paint.values[1]; pr4[2] = pcc->paint.values[2]; pr4[3] = pcc->paint.values[3]; break; default: pr4[0] = pr4[1] = pr4[2] = 0.0; pr4[3] = 1.0; } return 0; }
/* * Convert a CIEBasedDEFG color into device color. */ static int client_remap_CIEBasedDEFG(client_custom_color_params_t * pparams, const gs_client_color * pc, const gs_color_space * pcs, gx_device_color * pdc, const gs_imager_state * pis, gx_device * dev, gs_color_select_t select) { demo_color_space_data_t * pdata = (demo_color_space_data_t *)(pcs->pclient_color_space_data); frac cmyk[4]; int i; /*** Demonstrate method to convert to XYZ ***/ if (pdata->CIEtoXYZ_pis) { frac xyz[3]; cs_concretize_color(pc, pcs, xyz, pdata->CIEtoXYZ_pis); /* We don't really do anything with these values, but this */ /* is where a real client could convert to a device color */ if_debug7('|', "[c]client_remap CIEDEFG [%g, %g, %g] -> XYZ [%g, %g, %g]\n", pc->paint.values[0], pc->paint.values[1], pc->paint.values[2], pc->paint.values[3], frac2float(xyz[0]), frac2float(xyz[1]), frac2float(xyz[2])); } /* * For demo and debug purposes, make our colors a function of the * intensity of the given color value and the object type. The color * values could represent almost anything. However we are assuming * that they are CMYK values. */ for (i = 0; i < 4; i++) cmyk[i] = convert2frac(pc->paint.values[i], pcs->params.defg->RangeDEFG.ranges[i]); return client_remap_DeviceRGB(pparams, cmyk, pcs, pdc, pis, dev, select); }
static void cmyk_cs_to_spotn_cm(gx_device * dev, frac c, frac m, frac y, frac k, frac out[]) { /* TO_DO_DEVICEN This routine needs to include the effects of the SeparationOrder array */ xcf_device *xdev = (xcf_device *)dev; int n = xdev->separation_names.num_names; icmLuBase *luo = xdev->lu_cmyk; int i; if (luo != NULL) { double in[4]; double tmp[MAX_CHAN]; int outn = xdev->lu_cmyk_outn; in[0] = frac2float(c); in[1] = frac2float(m); in[2] = frac2float(y); in[3] = frac2float(k); luo->lookup(luo, tmp, in); for (i = 0; i < outn; i++) out[i] = float2frac(tmp[i]); for (; i < n + 4; i++) out[i] = 0; } else { /* If no profile given, assume CMYK */ out[0] = c; out[1] = m; out[2] = y; out[3] = k; for(i = 0; i < n; i++) /* Clear spot colors */ out[4 + i] = 0; } }
/* * Convert a CIEBasedA color into device color. */ static int client_remap_CIEBasedA(client_custom_color_params_t * pparams, const gs_client_color * pc, const gs_color_space * pcs, gx_device_color * pdc, const gs_imager_state * pis, gx_device * dev, gs_color_select_t select) { demo_color_space_data_t * pdata = (demo_color_space_data_t *)(pcs->pclient_color_space_data); frac gray = convert2frac(pc->paint.values[0], pcs->params.a->RangeA); /*** Demonstrate method to convert to XYZ ***/ if (pdata->CIEtoXYZ_pis) { frac xyz[3]; cs_concretize_color(pc, pcs, xyz, pdata->CIEtoXYZ_pis); /* We don't really do anything with these values, but this */ /* is where a real client could convert to a device color */ if_debug4('|', "[c]client_remap CIEA [%g] -> XYZ [%g, %g, %g]\n", pc->paint.values[0], frac2float(xyz[0]), frac2float(xyz[1]), frac2float(xyz[2])); } /* * For demo and debug purposes, make our colors a function of the * intensity of the given color value and the object type. */ return client_remap_DeviceGray(pparams, &gray, pcs, pdc, pis, dev, select); }
/* * Convert a ICCBased color into device color. */ static int client_remap_ICCBased(client_custom_color_params_t * pparams, const gs_client_color * pc, const gs_color_space * pcs, gx_device_color * pdc, const gs_imager_state * pis, gx_device * dev, gs_color_select_t select) { demo_color_space_data_t * pdata = (demo_color_space_data_t *)(pcs->pclient_color_space_data); frac frac_color[GS_CLIENT_COLOR_MAX_COMPONENTS]; int i, num_values = pcs->params.icc.picc_info->num_components; /*** Demonstrate method to convert to XYZ ***/ if (pdata->CIEtoXYZ_pis) { frac xyz[3]; cs_concretize_color(pc, pcs, xyz, pdata->CIEtoXYZ_pis); /* We don't really do anything with these values, but this */ /* is where a real client could convert to a device color */ if_debug6('|', "[c]client_remap ICCBased [%g, %g, %g] -> XYZ [%g, %g, %g]\n", pc->paint.values[0], pc->paint.values[1], pc->paint.values[2], frac2float(xyz[0]), frac2float(xyz[1]), frac2float(xyz[2])); } /* * For demo and debug purposes, make our colors a function of the * intensity of the given color value and the object type. The color * values could represent almost anything. However based upon the * number of color values, we are assuming that they are either * gray, RGB, or CMYK values. */ for (i = 0; i < num_values; i++) frac_color[i] = convert2frac(pc->paint.values[i], pcs->params.icc.picc_info->Range.ranges[i]); switch (num_values) { case 0: case 2: return_error(gs_error_rangecheck); case 1: return client_remap_DeviceGray(pparams, frac_color, pcs, pdc, pis, dev, select); case 3: return client_remap_DeviceRGB(pparams, frac_color, pcs, pdc, pis, dev, select); case 4: default: return client_remap_DeviceCMYK(pparams, frac_color, pcs, pdc, pis, dev, select); } }
/* Transform a CIEBased color to XYZ. */ static int cie_to_xyz(const double *in, double out[3], const gs_color_space *pcs, const gs_imager_state *pis) { gs_client_color cc; frac xyz[3]; int ncomp = gs_color_space_num_components(pcs); int i; for (i = 0; i < ncomp; ++i) cc.paint.values[i] = in[i]; cs_concretize_color(&cc, pcs, xyz, pis); out[0] = frac2float(xyz[0]); out[1] = frac2float(xyz[1]); out[2] = frac2float(xyz[2]); return 0; }
/* Note that this involves black generation and undercolor removal. */ void color_rgb_to_cmyk(frac r, frac g, frac b, const gs_imager_state * pis, frac cmyk[4], gs_memory_t *mem) { frac c = frac_1 - r, m = frac_1 - g, y = frac_1 - b; frac k = (c < m ? min(c, y) : min(m, y)); /* * The default UCR and BG functions are pretty arbitrary, * but they must agree with the ones in gs_init.ps. */ frac bg = (pis == NULL ? k : pis->black_generation == NULL ? frac_0 : gx_map_color_frac(pis, k, black_generation)); signed_frac ucr = (pis == NULL ? k : pis->undercolor_removal == NULL ? frac_0 : gx_map_color_frac(pis, k, undercolor_removal)); if (ucr == frac_1) cmyk[0] = cmyk[1] = cmyk[2] = 0; else if (ucr == frac_0) cmyk[0] = c, cmyk[1] = m, cmyk[2] = y; else { if (!gs_currentcpsimode(mem)) { /* C = max(0.0, min(1.0, 1 - R - UCR)), etc. */ signed_frac not_ucr = (ucr < 0 ? frac_1 + ucr : frac_1); cmyk[0] = (c < ucr ? frac_0 : c > not_ucr ? frac_1 : c - ucr); cmyk[1] = (m < ucr ? frac_0 : m > not_ucr ? frac_1 : m - ucr); cmyk[2] = (y < ucr ? frac_0 : y > not_ucr ? frac_1 : y - ucr); } else { /* Adobe CPSI method */ /* C = max(0.0, min(1.0, 1 - R / (1 - UCR))), etc. */ float denom = frac2float(frac_1 - ucr); /* unscaled */ float v; v = (float)frac_1 - r / denom; /* unscaled */ cmyk[0] = (is_fneg(v) ? frac_0 : v >= (float)frac_1 ? frac_1 : (frac) v); v = (float)frac_1 - g / denom; /* unscaled */ cmyk[1] = (is_fneg(v) ? frac_0 : v >= (float)frac_1 ? frac_1 : (frac) v); v = (float)frac_1 - b / denom; /* unscaled */ cmyk[2] = (is_fneg(v) ? frac_0 : v >= (float)frac_1 ? frac_1 : (frac) v); } } cmyk[3] = bg; if_debug7('c', "[c]RGB 0x%x,0x%x,0x%x -> CMYK 0x%x,0x%x,0x%x,0x%x\n", r, g, b, cmyk[0], cmyk[1], cmyk[2], cmyk[3]); }
/* Convert RGB to HSB. */ static void color_rgb_to_hsb(floatp r, floatp g, floatp b, float hsb[3]) { frac red = float2frac(r), green = float2frac(g), blue = float2frac(b); #define rhue hsb[0] #define rsat hsb[1] #define rbri hsb[2] if (red == green && green == blue) { rhue = 0; /* arbitrary */ rsat = 0; rbri = r; /* pick any one */ } else { /* Convert rgb to hsb */ frac V, Temp, diff; long H; V = (red > green ? red : green); if (blue > V) V = blue; Temp = (red > green ? green : red); if (blue < Temp) Temp = blue; diff = V - Temp; if (V == red) H = (green - blue) * frac_1_long / diff; else if (V == green) H = (blue - red) * frac_1_long / diff + 2 * frac_1_long; else /* V == blue */ H = (red - green) * frac_1_long / diff + 4 * frac_1_long; if (H < 0) H += 6 * frac_1_long; rhue = H / (frac_1 * 6.0); rsat = diff / (float)V; rbri = frac2float(V); } #undef rhue #undef rsat #undef rbri }
void flightControl(void) { // this should run at 500Hz if ((micros() - elapsed_micros) >= (2000)) { elapsed_micros = micros(); // flicker the leds to measure correct speed GPIOA->ODR ^= 1<<15; GPIOB->ODR ^= 1<<2; // read gyro gyro_read(&gyro_data); // controls /* throttle = payload[0] * 0xff; yaw = payload[1] * 0xff; pitch = payload[3] * 0xff; roll = payload[4] * 0xff; // trims yaw_trim = payload[2]; pitch_trim = payload[5]; roll_trim = payload[6]; */ // convert channels for crazyflie PID: #if 0 rollRateDesired = (float) (payload[4] - 128.0) * 256; pitchRateDesired = (float) (payload[3] - 128.0) * 256; yawRateDesired = (float) (payload[1] - 128.0) * 256; #else actuatorThrust = payload[0] * 256; // Roll, aka aileron, float +- 50.0 in degrees s32 f_roll = (payload[4] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400); frac2float(f_roll, &rollRateDesired); // Pitch, aka elevator, float +- 50.0 degrees s32 f_pitch = (payload[3] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400); frac2float(f_pitch, &pitchRateDesired); // Thrust, aka throttle 0..65535, working range 5535..65535 // No space for overshoot here, hard limit Channel3 by -10000..10000 /* s32 ch = payload[0] * 0xff; if (ch < CHAN_MIN_VALUE) { ch = CHAN_MIN_VALUE; } else if (ch > CHAN_MAX_VALUE) { ch = CHAN_MAX_VALUE; } actuatorThrust = ch*3L + 35535L; */ // Yaw, aka rudder, float +- 400.0 deg/s s32 f_yaw = (payload[1] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400); frac2float(f_yaw, &yawRateDesired); #endif // gyro.* == *rateActual == Data measured by IMU // *rateDesired == Data from RX controllerCorrectRatePID(-gyro_data.x, gyro_data.y, -gyro_data.z, rollRateDesired, pitchRateDesired, yawRateDesired); //#define TUNE_ROLL if (actuatorThrust > 0) { #if defined(TUNE_ROLL) distributePower(actuatorThrust, rollOutput, 0, 0); #elif defined(TUNE_PITCH) distributePower(actuatorThrust, 0, pitchOutput, 0); #elif defined(TUNE_YAW) distributePower(actuatorThrust, 0, 0, -yawOutput); #else distributePower(actuatorThrust, rollOutput, pitchOutput, -yawOutput); #endif } else { distributePower(0, 0, 0, 0); pidReset(&pidRollRate); pidReset(&pidPitchRate); pidReset(&pidYawRate); } } #if 0 m0_val = actuatorThrust; m1_val = actuatorThrust; m2_val = actuatorThrust; m3_val = ((yawOutput * 1000) / 0xffff) + 1000; TIM2->CCR4 = m0_val; // Motor "B" TIM1->CCR1 = m1_val; // Motor "L" TIM1->CCR4 = m2_val; // Motor "R" TIM16->CCR1 = m3_val; // Motor "F" #endif }
/* Convert HSB to RGB. */ static void color_hsb_to_rgb(floatp hue, floatp saturation, floatp brightness, float rgb[3]) { if (saturation == 0) { rgb[0] = rgb[1] = rgb[2] = brightness; } else { /* Convert hsb to rgb. */ /* We rely on the fact that the product of two */ /* fracs fits into an unsigned long. */ floatp h6 = hue * 6; ulong V = float2frac(brightness); /* force arithmetic to long */ frac S = float2frac(saturation); int I = (int)h6; ulong F = float2frac(h6 - I); /* ditto */ /* M = V*(1-S), N = V*(1-S*F), K = V*(1-S*(1-F)) = M-N+V */ frac M = V * (frac_1_long - S) / frac_1_long; frac N = V * (frac_1_long - S * F / frac_1_long) / frac_1_long; frac K = M - N + V; frac R, G, B; switch (I) { default: R = V; G = K; B = M; break; case 1: R = N; G = V; B = M; break; case 2: R = M; G = V; B = K; break; case 3: R = M; G = N; B = V; break; case 4: R = K; G = M; B = V; break; case 5: R = V; G = M; B = N; break; } rgb[0] = frac2float(R); rgb[1] = frac2float(G); rgb[2] = frac2float(B); #ifdef DEBUG if (gs_debug_c('c')) { dlprintf7("[c]hsb(%g,%g,%g)->VSFI(%ld,%d,%ld,%d)->\n", hue, saturation, brightness, V, S, F, I); dlprintf6(" RGB(%d,%d,%d)->rgb(%g,%g,%g)\n", R, G, B, rgb[0], rgb[1], rgb[2]); } #endif } }
/* this procedure is exported for the benefit of gsicc.c */ int gx_cie_real_remap_finish(cie_cached_vector3 vec3, frac * pconc, const gs_imager_state * pis, const gs_color_space *pcs) { const gs_cie_render *pcrd = pis->cie_render; const gx_cie_joint_caches *pjc = pis->cie_joint_caches; const gs_const_string *table = pcrd->RenderTable.lookup.table; int tabc[3]; /* indices for final EncodeABC lookup */ /* Apply DecodeLMN, MatrixLMN(decode), and MatrixPQR. */ if (!pjc->skipDecodeLMN) cie_lookup_map3(&vec3 /* LMN => PQR */, &pjc->DecodeLMN, "Decode/MatrixLMN+MatrixPQR"); /* Apply TransformPQR, MatrixPQR', and MatrixLMN(encode). */ if (!pjc->skipPQR) cie_lookup_map3(&vec3 /* PQR => LMN */, &pjc->TransformPQR, "Transform/Matrix'PQR+MatrixLMN"); /* Apply EncodeLMN and MatrixABC(encode). */ if (!pjc->skipEncodeLMN) cie_lookup_map3(&vec3 /* LMN => ABC */, &pcrd->caches.EncodeLMN, "EncodeLMN+MatrixABC"); /* MatrixABCEncode includes the scaling of the EncodeABC */ /* cache index. */ #define SET_TABC(i, t)\ BEGIN\ tabc[i] = cie_cached2int(vec3 /*ABC*/.t - pcrd->EncodeABC_base[i],\ _cie_interpolate_bits);\ if ((uint)tabc[i] > (gx_cie_cache_size - 1) << _cie_interpolate_bits)\ tabc[i] = (tabc[i] < 0 ? 0 :\ (gx_cie_cache_size - 1) << _cie_interpolate_bits);\ END SET_TABC(0, u); SET_TABC(1, v); SET_TABC(2, w); #undef SET_TABC if (table == 0) { /* * No further transformation. * The final mapping step includes both restriction to * the range [0..1] and conversion to fracs. */ #define EABC(i)\ cie_interpolate_fracs(pcrd->caches.EncodeABC[i].fixeds.fracs.values, tabc[i]) pconc[0] = EABC(0); pconc[1] = EABC(1); pconc[2] = EABC(2); #undef EABC return 3; } else { /* * Use the RenderTable. */ int m = pcrd->RenderTable.lookup.m; #define RT_LOOKUP(j, i) pcrd->caches.RenderTableT[j].fracs.values[i] #ifdef CIE_RENDER_TABLE_INTERPOLATE /* * The final mapping step includes restriction to the * ranges [0..dims[c]] as ints with interpolation bits. */ fixed rfix[3]; const int s = _fixed_shift - _cie_interpolate_bits; #define EABC(i)\ cie_interpolate_fracs(pcrd->caches.EncodeABC[i].fixeds.ints.values, tabc[i]) #define FABC(i, s)\ ((s) > 0) ? (EABC(i) << (s)) : (EABC(i) >> -(s)) rfix[0] = FABC(0, s); rfix[1] = FABC(1, s); rfix[2] = FABC(2, s); #undef FABC #undef EABC if_debug6('c', "[c]ABC=%g,%g,%g => iabc=%g,%g,%g\n", cie_cached2float(vec3.u), cie_cached2float(vec3.v), cie_cached2float(vec3.w), fixed2float(rfix[0]), fixed2float(rfix[1]), fixed2float(rfix[2])); gx_color_interpolate_linear(rfix, &pcrd->RenderTable.lookup, pconc); if_debug3('c', "[c] interpolated => %g,%g,%g\n", frac2float(pconc[0]), frac2float(pconc[1]), frac2float(pconc[2])); if (!pcrd->caches.RenderTableT_is_identity) { /* Map the interpolated values. */ #define frac2cache_index(v) frac2bits(v, gx_cie_log2_cache_size) pconc[0] = RT_LOOKUP(0, frac2cache_index(pconc[0])); pconc[1] = RT_LOOKUP(1, frac2cache_index(pconc[1])); pconc[2] = RT_LOOKUP(2, frac2cache_index(pconc[2])); if (m > 3) pconc[3] = RT_LOOKUP(3, frac2cache_index(pconc[3])); #undef frac2cache_index } #else /* !CIE_RENDER_TABLE_INTERPOLATE */ /* * The final mapping step includes restriction to the ranges * [0..dims[c]], plus scaling of the indices in the strings. */ #define RI(i)\ pcrd->caches.EncodeABC[i].ints.values[tabc[i] >> _cie_interpolate_bits] int ia = RI(0); int ib = RI(1); /* pre-multiplied by m * NC */ int ic = RI(2); /* pre-multiplied by m */ const byte *prtc = table[ia].data + ib + ic; /* (*pcrd->RenderTable.T)(prtc, m, pcrd, pconc); */ if_debug6('c', "[c]ABC=%g,%g,%g => iabc=%d,%d,%d\n", cie_cached2float(vec3.u), cie_cached2float(vec3.v), cie_cached2float(vec3.w), ia, ib, ic); if (pcrd->caches.RenderTableT_is_identity) { pconc[0] = byte2frac(prtc[0]); pconc[1] = byte2frac(prtc[1]); pconc[2] = byte2frac(prtc[2]); if (m > 3) pconc[3] = byte2frac(prtc[3]); } else { #if gx_cie_log2_cache_size == 8 # define byte2cache_index(b) (b) #else # if gx_cie_log2_cache_size > 8 # define byte2cache_index(b)\ ( ((b) << (gx_cie_log2_cache_size - 8)) +\ ((b) >> (16 - gx_cie_log2_cache_size)) ) # else /* < 8 */ # define byte2cache_index(b) ((b) >> (8 - gx_cie_log2_cache_size)) # endif #endif pconc[0] = RT_LOOKUP(0, byte2cache_index(prtc[0])); pconc[1] = RT_LOOKUP(1, byte2cache_index(prtc[1])); pconc[2] = RT_LOOKUP(2, byte2cache_index(prtc[2])); if (m > 3) pconc[3] = RT_LOOKUP(3, byte2cache_index(prtc[3])); #undef byte2cache_index } #endif /* !CIE_RENDER_TABLE_INTERPOLATE */ #undef RI #undef RT_LOOKUP return m; } }
static void send_cmd_packet() { s32 f_roll; s32 f_pitch; s32 f_yaw; s32 thrust_truncated; u16 thrust; struct CommanderPacker { float roll; float pitch; float yaw; uint16_t thrust; } __attribute__((packed)) cpkt; // Channels in AETR order // Roll, aka aileron, float +- 50.0 in degrees // float roll = -(float) Channels[0]*50.0/10000; f_roll = -Channels[0] * FRAC_SCALE / (10000 / 50); // Pitch, aka elevator, float +- 50.0 degrees //float pitch = -(float) Channels[1]*50.0/10000; f_pitch = -Channels[1] * FRAC_SCALE / (10000 / 50); // Thrust, aka throttle 0..65535, working range 5535..65535 // No space for overshoot here, hard limit Channel3 by -10000..10000 thrust_truncated = Channels[2]; if (thrust_truncated < CHAN_MIN_VALUE) { thrust_truncated = CHAN_MIN_VALUE; } else if (thrust_truncated > CHAN_MAX_VALUE) { thrust_truncated = CHAN_MAX_VALUE; } thrust = thrust_truncated*3L + 35535L; // Crazyflie needs zero thrust to unlock if (thrust < 6000) cpkt.thrust = 0; else cpkt.thrust = thrust; // Yaw, aka rudder, float +- 400.0 deg/s // float yaw = -(float) Channels[3]*400.0/10000; f_yaw = - Channels[3] * FRAC_SCALE / (10000 / 400); frac2float(f_yaw, &cpkt.yaw); // Switch on/off? if (Channels[4] >= 0) { frac2float(f_roll, &cpkt.roll); frac2float(f_pitch, &cpkt.pitch); } else { // Rotate 45 degrees going from X to + mode or opposite. // 181 / 256 = 0.70703125 ~= sqrt(2) / 2 s32 f_x_roll = (f_roll + f_pitch) * 181 / 256; frac2float(f_x_roll, &cpkt.roll); s32 f_x_pitch = (f_pitch - f_roll) * 181 / 256; frac2float(f_x_pitch, &cpkt.pitch); } // Construct and send packet tx_packet[0] = crtp_create_header(CRTP_PORT_COMMANDER, 0); // Commander packet to channel 0 memcpy(&tx_packet[1], (char*) &cpkt, sizeof(cpkt)); tx_payload_len = 1 + sizeof(cpkt); send_packet(); // Print channels every 2 seconds or so if ((packet_counter & 0xFF) == 1) { dbgprintf("Raw channels: %d, %d, %d, %d, %d, %d, %d, %d\n", Channels[0], Channels[1], Channels[2], Channels[3], Channels[4], Channels[5], Channels[6], Channels[7]); dbgprintf("Roll %d, pitch %d, yaw %d, thrust %d\n", (int) f_roll*100/FRAC_SCALE, (int) f_pitch*100/FRAC_SCALE, (int) f_yaw*100/FRAC_SCALE, (int) thrust); } }