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);
}
}
