static void scan_SubTable(fz_context *ctx, struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
FT_Offset Coverage;
struct SingleSubst_st *t;
t = &ret->SubTable[ret->SubTableCount].SingleSubst;
t->SubstFormat = BYTE2(s);
Coverage = BYTE2(s);
scan_Coverage(ctx, ret, top + Coverage);
switch (t->SubstFormat)
{
case 1: /* SingleSubstFormat1 */
t->DeltaGlyphID = BYTE2(s);
break;
case 2: /* SingleSubstFormat2 */
t->GlyphCount = BYTE2(s);
t->Substitute = fz_calloc(ctx, t->GlyphCount, sizeof(t->Substitute[0]));
memset(t->Substitute, 0, t->GlyphCount * sizeof(t->Substitute[0]));
for (i = 0; i < t->GlyphCount; i++)
t->Substitute[i] = BYTE2(s);
break;
default:
fz_warn(ctx, "scan_SubTable: unknown SubstFormat (%d).", t->SubstFormat);
}
}
static void write_multibyte(long i)
{
if (BYTE1(i) != 0) buffer[last++] = BYTE1(i);
if (BYTE2(i) != 0) buffer[last++] = BYTE2(i);
/* always */ buffer[last++] = BYTE3(i);
/* always */ buffer[last++] = BYTE4(i);
}
u8_t* readPicture(u32_t n) {
u8_t args[] = {0x0C, 0x0, //帧类型 0:当前帧 1:下一帧
0x0A, //操作方式 0x0F(DMA)
0, 0, frameptr >> 8, frameptr & 0xFF, //起始地址 4字节
0, 0, BYTE2(n), BYTE1(n), //数据长度 4字节
BYTE2(CAMERADELAY), BYTE1(CAMERADELAY) //延时时间 2字节
};
uint32_t * vtuDownsampleImageRGBA(const uint32_t *tex)
{
uint32_t *smallTex = (uint32_t *)malloc(c.pageDimension * c.pageDimension);
assert(smallTex);
for (uint16_t x = 0; x < c.pageDimension / 2; x++)
{
for (uint16_t y = 0; y < c.pageDimension / 2; y++)
{
#ifdef COLOR_CODE_MIPPED_PHYSTEX
smallTex[y * (c.pageDimension / 2) + x] = (255 << 24) + (0 << 16) + (0 << 8) + 255;
#else
uint32_t pix1 = tex[(y*2) * c.pageDimension + (x*2)];
uint32_t pix2 = tex[(y*2+1) * c.pageDimension + (x*2)];
uint32_t pix3 = tex[(y*2) * c.pageDimension + (x*2+1)];
uint32_t pix4 = tex[(y*2+1) * c.pageDimension + (x*2+1)];
uint32_t b1 = BYTE1(pix1) + BYTE1(pix2) + BYTE1(pix3) + BYTE1(pix4);
uint32_t b2 = BYTE2(pix1) + BYTE2(pix2) + BYTE2(pix3) + BYTE2(pix4);
uint32_t b3 = BYTE3(pix1) + BYTE3(pix2) + BYTE3(pix3) + BYTE3(pix4);
uint32_t b4 = BYTE4(pix1) + BYTE4(pix2) + BYTE4(pix3) + BYTE4(pix4);
smallTex[y * (c.pageDimension / 2) + x] = ((b4 / 4) << 24) + ((b3 / 4) << 16) + ((b2 / 4) << 8) + (b1 / 4); // ARGB
#endif
}
}
return smallTex;
}
static int isInIndex(FT_Bytes s, int index)
{
int i, count;
if (s == NULL)
return 0;
count = BYTE2(s);
for (i = 0; i < count; i++)
if (index == BYTE2(s))
return 1;
return 0;
}
static void scan_LookupList(fz_context *ctx, struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
int LookupCount;
LookupCount = BYTE2(s);
for (i = 0; i < LookupCount; i++)
{
FT_Bytes t = top + BYTE2(s);
if (isInIndex(ret->vertLookup, i))
scan_Lookup(ctx, ret, t);
}
}
static int put_multibyte(long c, FILE *fp) {
#ifdef WIN32
if (sjisterminal) {
const int fd = fileno(fp);
if ((fd == fileno(stdout) || fd == fileno(stderr)) && _isatty(fd)) {
HANDLE hStdout;
DWORD ret, wclen;
UINT cp;
wchar_t buff[2];
char str[4];
int mblen;
if (fd == fileno(stdout))
hStdout = GetStdHandle(STD_OUTPUT_HANDLE);
else
hStdout = GetStdHandle(STD_ERROR_HANDLE);
mblen=0;
if (BYTE1(c) != 0) str[mblen++]=BYTE1(c);
if (BYTE2(c) != 0) str[mblen++]=BYTE2(c);
if (BYTE3(c) != 0) str[mblen++]=BYTE3(c);
/* always */ str[mblen++]=BYTE4(c);
#define CP_932 932
#define CP_UTF8 65001
if (is_internalUPTEX())
cp = CP_UTF8;
else
cp = CP_932;
if (MultiByteToWideChar(cp, 0, str, mblen, buff, 2) == 0)
return EOF;
wclen = mblen > 3 ? 2 : 1;
if (WriteConsoleW(hStdout, buff, wclen, &ret, NULL) == 0)
return EOF;
return BYTE4(c);
}
}
#endif
if (BYTE1(c) != 0 && putc(BYTE1(c), fp) == EOF) return EOF;
if (BYTE2(c) != 0 && putc(BYTE2(c), fp) == EOF) return EOF;
if (BYTE3(c) != 0 && putc(BYTE3(c), fp) == EOF) return EOF;
/* always */ return putc(BYTE4(c), fp);
}
static void scan_ScriptList(struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
int ScriptCount;
ScriptCount = BYTE2(s);
for (i = 0; i < ScriptCount; i++)
{
FT_Tag ScriptTag = BYTE4(s);
FT_Bytes t = top + BYTE2(s);
if (ScriptTag == TAG_KANA)
scan_Script(ret, t, ScriptTag);
}
}
void ANO_DT::Send_Status(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
vs16 _temp;
_temp = (int)(imu.angle.x*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.y*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.z*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
vs32 _temp2 = 0;//UltraAlt * 100;
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void ANO_DT::Send_Senser2(s32 alt_bar,u16 alt_csb)
{
u8 _cnt=0;
vs32 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x07;
data_to_send[_cnt++]=0;
_temp = alt_bar;
data_to_send[_cnt++]=BYTE3(_temp);
data_to_send[_cnt++]=BYTE2(_temp);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = alt_csb;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
Send_Data(data_to_send, _cnt);
}
static void showDevice(usb_device_t* usbDev)
{
textColor(IMPORTANT);
if (usbDev->usbSpec == 0x0100 || usbDev->usbSpec == 0x0110 || usbDev->usbSpec == 0x0200 || usbDev->usbSpec == 0x0300)
{
textColor(SUCCESS);
printf("\nUSB %y.%y\t", BYTE2(usbDev->usbSpec), BYTE1(usbDev->usbSpec)); // e.g. 0x0210 means 2.10
textColor(TEXT);
}
else
{
textColor(ERROR);
printf("\nInvalid USB version %u.%u !", BYTE2(usbDev->usbSpec), BYTE1(usbDev->usbSpec));
textColor(TEXT);
return;
}
if (usbDev->usbClass == 0x09)
{
switch (usbDev->usbProtocol)
{
case 0:
printf("Full speed USB hub");
break;
case 1:
printf("Hi-speed USB hub with single TT");
break;
case 2:
printf("Hi-speed USB hub with multiple TTs");
break;
}
}
printf("\nendpoint 0 mps: %u byte.", usbDev->endpoints[0].mps); // MPS0, must be 8,16,32,64
#ifdef _USB_TRANSFER_DIAGNOSIS_
printf("vendor: %xh\n", usbDev->vendor);
printf("product: %xh\t", usbDev->product);
printf("release number: %u.%u\n", BYTE2(usbDev->releaseNumber), BYTE1(usbDev->releaseNumber));
printf("manufacturer: %xh\t", usbDev->manufacturerStringID);
printf("product: %xh\n", usbDev->productStringID);
printf("serial number: %xh\t", usbDev->serNumberStringID);
printf("number of config.: %u\n", usbDev->numConfigurations); // number of possible configurations
printf("numInterfaceMSD: %u\n", usbDev->numInterfaceMSD);
#endif
textColor(TEXT);
}
void
bufPutSInt(Buffer b, ULong i)
{
bufAdd1(b, BYTE0(i));
bufAdd1(b, BYTE1(i));
bufAdd1(b, BYTE2(i));
bufAdd1(b, BYTE3(i));
}
static void fprint_euc_char(FILE *fp, const char a, const char b)
{
if (is_internalUPTEX()) { /* convert a character from EUC to UTF8 */
int k = 0;
unsigned char str[5];
int chr = (unsigned char)a<<8 | (unsigned char)b;
chr = (chr==0xffff) ? U_REPLACEMENT_CHARACTER : JIStoUCS2(chr & 0x7f7f);
chr = UCStoUTF8(chr);
/* if (BYTE1(chr) != 0) str[k++] = BYTE1(chr); */ /* do not happen */
if (BYTE2(chr) != 0) str[k++] = BYTE2(chr);
if (BYTE3(chr) != 0) str[k++] = BYTE3(chr);
str[k++] = BYTE4(chr);
str[k++] = '\0';
fprintf(fp,"%s",str);
}
else
fprintf(fp,"%c%c",a,b);
}
static void scan_Script(struct ft2vert_st *ret, const FT_Bytes top, const FT_Tag ScriptTag)
{
int i;
FT_Bytes s = top;
FT_Offset DefaultLangSys;
int LangSysCount;
DefaultLangSys = BYTE2(s);
if (DefaultLangSys != 0)
scan_LangSys(ret, top + DefaultLangSys, ScriptTag);
LangSysCount = BYTE2(s);
for (i = 0; i < LangSysCount; i++)
{
FT_Tag LangSysTag = BYTE4(s);
FT_Bytes t = top + BYTE2(s);
if (LangSysTag == TAG_JAN)
scan_LangSys(ret, t, ScriptTag);
}
}
void __fastcall ISave_2(int v)
{
*(_BYTE *)tbuff = _HIBYTE(v);
tbuff = (char *)tbuff + 1;
*(_BYTE *)tbuff = BYTE2(v);
tbuff = (char *)tbuff + 1;
*(_BYTE *)tbuff = BYTE1(v);
tbuff = (char *)tbuff + 1;
*(_BYTE *)tbuff = v;
tbuff = (char *)tbuff + 1;
}
DWORD R0GetDescBase(PDESCRIPTOR pDesc)
{
DWORD dwBase;
WORD0(dwBase) = pDesc->Base_0_15;
BYTE2(dwBase) = pDesc->Base_16_23;
BYTE3(dwBase) = pDesc->Base_24_31;
// To do: Account for expand-down segments
return dwBase;
}
static void scan_Coverage(fz_context *ctx, struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
struct Coverage_st *t;
t = &ret->SubTable[ret->SubTableCount].Coverage;
t->CoverageFormat = BYTE2(s);
switch (t->CoverageFormat)
{
case 1:
t->GlyphCount = BYTE2(s);
t->GlyphArray = fz_calloc(ctx, t->GlyphCount, sizeof(t->GlyphArray[0]));
memset(t->GlyphArray, 0, t->GlyphCount * sizeof(t->GlyphArray[0]));
for (i = 0; i < t->GlyphCount; i++)
t->GlyphArray[i] = BYTE2(s);
break;
case 2:
t->RangeCount = BYTE2(s);
t->RangeRecord = fz_calloc(ctx, t->RangeCount, sizeof(t->RangeRecord[0]));
memset(t->RangeRecord, 0, t->RangeCount * sizeof(t->RangeRecord[0]));
for (i = 0; i < t->RangeCount; i++)
{
t->RangeRecord[i].Start = BYTE2(s);
t->RangeRecord[i].End = BYTE2(s);
s += 2; /* drop StartCoverageIndex */
}
break;
default:
fz_warn(ctx, "scan_Coverage: unknown CoverageFormat (%d).", t->CoverageFormat);
return;
}
ret->SubTableCount++;
}
// Function to play sound from the EEPROM
void play_sound(short rate, unsigned long offset, unsigned long length) {
unsigned int wave_data;
// Wake up EEPROM - with power-up delay
PORTC.F0 = 1;
PORTC.F0 = 0;
spi_write(0xAB);
PORTC.F0 = 1;
delay_us(100);
// Setup the EEPROM
PORTC.F0 = 0;
PORTC.F2 = 1; // Unhold EEPROM
spi_write(0x03); // EEPROM read command
spi_write(BYTE2(offset));
spi_write(BYTE1(offset));
spi_write(BYTE0(offset));
// Process all bytes in sound file
for (wave_scan = 0x000000; wave_scan < length; wave_scan++) {
// Retrieve a byte of audio data
PORTC.F2 = 1; // Unhold EEPROM
wave_data = (SPI_Read(0x00) << 4); // Read EEPROM byte
PORTC.F2 = 0; // Hold EEPROM
// Write audio data to DAC
PORTC.F1 = 0;
spi_write(BYTE1(wave_data) | 0x10);
spi_write(BYTE0(wave_data));
PORTC.F1 = 1;
// Set delays for different sampling rates
switch (rate) {
case FREQ_8000: delay_us(0x65); break;
case FREQ_11025: delay_us(0x41); break;
case FREQ_22050: delay_us(0x12); break;
}
}
// Set DAC voltage output to normalized level
PORTC.F1 = 0;
spi_write(0x18);
spi_write(0x00);
PORTC.F1 = 1;
// Shutdown EEPROM - with power-down delay
PORTC.F0 = 1;
PORTC.F0 = 0;
spi_write(0xB9);
PORTC.F0 = 1;
delay_us(100);
}
static void scan_FeatureList(struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
int FeatureCount;
FeatureCount = BYTE2(s);
for (i = 0; i < FeatureCount; i++)
{
FT_Tag FeatureTag = BYTE4(s);
FT_Offset Feature = BYTE2(s);
if (isInIndex(ret->kanaFeature, i))
{
switch (FeatureTag)
{
case TAG_VERT: ret->vertLookup = top + Feature + 2; break;
case TAG_VRT2: ret->vrt2Lookup = top + Feature + 2; break;
}
}
}
}
void Send_DEBUG(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
float data = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xF1;
data_to_send[_cnt++]=0;
data = Acc.z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = IMU_QCF.Acc_LPF2nd.lastout_z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = IMU_QCF.yaw;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.x;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.y;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
static void scan_Lookup(fz_context *ctx, struct ft2vert_st *ret, const FT_Bytes top)
{
int i;
FT_Bytes s = top;
FT_UShort LookupType;
FT_UShort LookupFlag;
FT_UShort SubTableCount;
FT_UShort SubTable;
LookupType = BYTE2(s);
LookupFlag = BYTE2(s);
SubTableCount = BYTE2(s);
SubTable = BYTE2(s);
ret->SubTable = fz_calloc(ctx, SubTableCount, sizeof(ret->SubTable[0]));
memset(ret->SubTable, 0, SubTableCount * sizeof(ret->SubTable[0]));
for (i = 0; i < SubTableCount; i++)
scan_SubTable(ctx, ret, top + SubTable);
if (ret->SubTableCount != SubTableCount)
fz_warn(ctx, "scan_Lookup: SubTableCount (=%d) is not expected (=%d).\n",
ret->SubTableCount, SubTableCount);
}
static void scan_GSUB_Header(fz_context *ctx, struct ft2vert_st *ret, const FT_Bytes top)
{
FT_Bytes s = top;
FT_Fixed Version;
FT_Offset ScriptList;
FT_Offset FeatureList;
FT_Offset LookupList;
Version = BYTE4(s);
ScriptList = BYTE2(s);
FeatureList = BYTE2(s);
LookupList = BYTE2(s);
if (Version != 0x00010000)
fz_warn(ctx, "GSUB Version (=%.1f) is not 1.0\n", (double)Version / 0x10000);
scan_ScriptList(ret, top + ScriptList);
scan_FeatureList(ret, top + FeatureList);
/* vrt2 has higher priority over vert */
if (ret->vrt2Lookup != NULL)
ret->vertLookup = ret->vrt2Lookup;
scan_LookupList(ctx, ret, top + LookupList);
}
void Data_Send_Status(T_RC_Status *data)
{
rt_uint8_t _cnt=0;
data_to_send[_cnt++]=0XAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
rt_int16_t _temp;
_temp = data->ANGLE.rol*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->ANGLE.pit*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->ANGLE.yaw*100;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = data->ALT_CSB;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(data->ALT_PRS);
data_to_send[_cnt++]=BYTE2(data->ALT_PRS);
data_to_send[_cnt++]=BYTE1(data->ALT_PRS);
data_to_send[_cnt++]=BYTE0(data->ALT_PRS);
if(data->ARMED==0)
data_to_send[_cnt++]=0xA0; //Ëø¶¨
else if(data->ARMED==1)
data_to_send[_cnt++]=0xA1;
data_to_send[3] = _cnt-4;
rt_uint8_t sum = 0;
for(rt_uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
NRF24L01_TxPacket(data_to_send);
}
void Data_Send_RCData(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x03;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = BYTE1(Rc_D.THROTTLE);
data_to_send[_cnt++] = BYTE0(Rc_D.THROTTLE);
data_to_send[_cnt++] = BYTE1(Rc_D.YAW);
data_to_send[_cnt++] = BYTE0(Rc_D.YAW);
data_to_send[_cnt++] = BYTE1(Rc_D.ROLL);
data_to_send[_cnt++] = BYTE0(Rc_D.ROLL);
data_to_send[_cnt++] = BYTE1(Rc_D.PITCH);
data_to_send[_cnt++] = BYTE0(Rc_D.PITCH);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX1);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX1);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX2);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX2);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX3);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX3);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX4);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX4);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX5);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX5);
data_to_send[_cnt++] = BYTE1(Rc_D.AUX6);
data_to_send[_cnt++] = BYTE0(Rc_D.AUX6);
data_to_send[_cnt++] = BYTE3(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE2(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE1(Alt_ultrasonic);
data_to_send[_cnt++] = BYTE0(Alt_ultrasonic);
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
bool Communication::SendCopterState(float angle_rol, float angle_pit, float angle_yaw, s32 Hight, u8 fly_model, u8 armed)
{
u8 _cnt=0;
vs16 _temp;
vs32 _temp2 = Hight;
u8 data_to_send[30];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(angle_rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[_cnt++] = fly_model;
data_to_send[_cnt++] = armed;
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
//发送
usart.SendData(data_to_send, _cnt);
return true;
}
void sound(uint32_t frequency)
{
// set commandregister to 0xB6 = 10 11 011 0 // Select channel, Access mode, Operating mode, BCD/Binary mode
outportb(COMMANDREGISTER, COUNTER_2 | RW_HI_LO_MODE | SQUAREWAVE); // x86 offers only binary mode (no BCD)
// calculate our divisor
uint16_t divisor = TIMECOUNTER_i8254_FREQU / frequency; //divisor must fit into 16 bits; PIT (programable interrupt timer)
// send divisor
outportb(COUNTER_2_DATAPORT, BYTE1(divisor));
outportb(COUNTER_2_DATAPORT, BYTE2(divisor));
// sound on
uint8_t temp = inportb(COUNTER_2_CONTROLPORT);
if (temp != (temp | (AUX_GATE_2 | AUX_OUT_2)))
{
outportb(COUNTER_2_CONTROLPORT, temp | (AUX_GATE_2 | AUX_OUT_2));
}
currentTask->speaker = true;
}
void Data_Send_Status(void)
{
u8 _cnt = 0;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0xAA;
data_to_send[_cnt++] = 0x01;
data_to_send[_cnt++] = 0;
vs16 _temp;
_temp = (int)(Att_Angle.rol * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(Att_Angle.pit * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = (int)(Att_Angle.yaw * 100);
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
_temp = Alt_ultrasonic;
data_to_send[_cnt++] = BYTE1(_temp);
data_to_send[_cnt++] = BYTE0(_temp);
vs32 _temp2 = Alt_bmp;
data_to_send[_cnt++] = BYTE3(_temp2);
data_to_send[_cnt++] = BYTE2(_temp2);
data_to_send[_cnt++] = BYTE1(_temp2);
data_to_send[_cnt++] = BYTE0(_temp2);
if (0 == RC_Control.ARMED)
data_to_send[_cnt++] = 0xA0;
else if (1 == RC_Control.ARMED)
data_to_send[_cnt++] = 0xA1;
data_to_send[3] = _cnt - 4;
u8 sum = 0;
for (u8 i = 0; i < _cnt; i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
#ifdef DATA_TRANSFER_USE_USART
Sys_sPrintf(Printf_USART, data_to_send, _cnt);
#else
NRF_TxPacket(data_to_send, _cnt);
#endif
}
/***** 发送飞机当前姿态*****/
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw)
{
uint8_t _cnt=0;
int16_t _temp;
int32_t _temp2 = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(angle_rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[3] = _cnt-4;
uint8_t sum = 0;
for(uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void Data_Send_Status(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
vs16 _temp;
_temp = (int)(Att_Angle.rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Att_Angle.pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Att_Angle.yaw*100);
//_temp = (int)(Mag_Heading*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
vs32 _temp2 = Alt;
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
if(Rc_C.ARMED==0) data_to_send[_cnt++]=0xA0; //Ëø¶¨
else if(Rc_C.ARMED==1) data_to_send[_cnt++]=0xA1;
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
#ifdef DATA_TRANSFER_USE_USART
Uart2_Put_Buf(data_to_send,_cnt);
#else
NRF_TxPacket(data_to_send,_cnt);
#endif
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, s32 alt, u8 fly_model, u8 armed)
{
u8 _cnt=0;
vs16 _temp;
vs32 _temp2 = alt;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(angle_rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[_cnt++] = fly_model;
data_to_send[_cnt++] = armed;
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}