void Sprite16 (short x, short y, short height, const unsigned short *sprite, void *vm_addr, short mode) {
TRACK("Sprite16");
Byte * Dest=(Byte *)vm_addr;
Byte * SpriteData=(Byte *)(void *)sprite;
int SpriteByteNum, ByteNum;
for(int sy=0; sy<height; sy++) {
for(int sx=0; sx<16; sx++) {
SpriteByteNum=((sy)*16)+(15-sx);
ByteNum=((sy+y)*ScreenState.XMax)+sx+x;
if(GetBit(SpriteData[SpriteByteNum/8], SpriteByteNum%8)!=0) {
if(mode==SPRT_XOR) {
if(GetBit(Dest[ByteNum/8], ByteNum%8)!=0) {
ClearBit(&Dest[ByteNum/8], ByteNum%8);
}
else
SetBit(&Dest[ByteNum/8], ByteNum%8);
}
if(mode==SPRT_OR || mode==SPRT_COPY) {
SetBit(&Dest[ByteNum/8], ByteNum%8);
}
}
else {
if(mode==SPRT_AND) {
SetBit(&Dest[ByteNum/8], ByteNum%8);
}
else if(mode==SPRT_COPY) {
ClearBit(&Dest[ByteNum/8], ByteNum%8);
}
}
}
}
}
void LCD_CommonFunc(unsigned char data)
{
#ifdef BUS_4BIT
unsigned char tmp;
tmp = PORT_DATA & 0x0F;
tmp |= (data & 0xF0);
PORT_DATA = tmp; //вывод старшей тетрады
SetBit(PORT_SIG_EN, EN);
_delay_us(LCD_SPEED);
ClearBit(PORT_SIG_EN, EN);
data = __swap_nibbles(data);
tmp = PORT_DATA & 0x0F;
tmp |= (data & 0xF0);
PORT_DATA = tmp; //вывод младшей тетрады
SetBit(PORT_SIG_EN, EN);
_delay_us(LCD_SPEED);
ClearBit(PORT_SIG_EN, EN);
#else
PORT_DATA = data; //вывод данных на шину индикатора
SetBit(PORT_SIG_EN, EN); //установка E в 1
_delay_us(LCD_SPEED);
ClearBit(PORT_SIG_EN, EN); //установка E в 0 - записывающий фронт
#endif
}
main()
{
float speed = *(float *)&persist.UserData[97]; // value stored is actually a float
float LastSpeed = *(float *)&persist.UserData[99]; // get last speed setting
float LastState = persist.UserData[98]; // get last state
if (LastState==1)
{
// if spindle was CW now we want CCW
// spin down
ClearBit(154);
ClearBit(155);
LastSpeed = 0.0;
Jog(SPINDLEAXIS,0);
while (!CheckDone(SPINDLEAXIS)) ;
}
// turn spindle on CCW and ramp to new speed
SetBit(155);
// spindle is already on, so ramp to new speed
if (LastSpeed != speed)
{
LastSpeed = speed;
Jog(SPINDLEAXIS,LastSpeed * FACTOR);
}
*(float *)&persist.UserData[99] = LastSpeed; // save the last speed
persist.UserData[98] = -1; // remember we are CCW
}
//функция чтения флага занятости
void LCDG_WaitFLAG_BUSY(void)
{
unsigned char stat;
DDRX_LCD_BUS = 0; //конфигурируем порт на вход
PORT_LCD_BUS = 0xff;
SetBit(PORT_LCD_CON, RD_WR); //выставляем управляющие сигналы
ClearBit(PORT_LCD_CON, AO);
asm("nop");
asm("nop");
do{
ClearBit(PORT_LCD_EN, EN);
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
stat = PIN_LCD_BUS; //считываем статусный байт
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
SetBit(PORT_LCD_EN, EN);
}
while((stat & (1<<FLAG_BUSY)) != 0); //сидим в цикле пока не сбросится флаг
DDRX_LCD_BUS = 0xff; //делаем порт снова выходом
PORT_LCD_BUS = 0;
}
main()
{
float speed = *(float *)&persist.UserData[SPEEDVAR]; // value stored is actually a float
float LastState = persist.UserData[STATEVAR]; // get last state
if (LastState==1)
{
// if spindle was CW now we want CCW
// spin down
ClearBit(SPINDLECW_BIT);
ClearBit(SPINDLECCW_BIT);
Jog(SPINDLEAXIS,0);
while (!CheckDone(SPINDLEAXIS)) ;
}
// turn spindle on CCW and ramp to new speed
SetBit(SPINDLECCW_BIT);
// spindle is already on, so ramp to new speed
Jog(SPINDLEAXIS,-speed * FACTOR);
printf("Jogging Spindle %f counts/sec\n",speed * FACTOR);
persist.UserData[STATEVAR] = -1; // remember we are CCW
}
bool BitTest::test_ClearBit()
{
int x = 2, y = 4;
ClearBit(x,1);
ClearBit(y,1);
return ( (x==0) && (y==4) );
}
// ----------------------------------------------------------------------------
void SetupCR0()
{
dword cr0 = __readcr0();
SetBit(cr0, 31); // Set PG 1 (Enable Paging)
ClearBit(cr0, 30); // Set CD 0 (Enable Cache)
ClearBit(cr0, 29); // Set NW 0 (Enable Writethrough)
SetBit(cr0, 16); // Set WP 1 (Enable Write Protect)
SetBit(cr0, 5); // Set NE 1 (Enable Internal FP Mode)
__writecr0(cr0);
}
main()
{
// spin down
ClearBit(SPINDLECW_BIT);
ClearBit(SPINDLECCW_BIT);
Jog(SPINDLEAXIS,0);
printf("Jogging Spindle Stop\n");
persist.UserData[STATEVAR] = 0; // remember we are Off
while (!CheckDone(SPINDLEAXIS)) ;
}
void booz2_analog_init_hw( void ) {
/* AD0 */
/* PCLK/4 ( 3.75MHz) - BURST - ON */
AD0CR = 0x03 << 8 | 1 << 16 | 1 << 21;
/* disable global interrupt */
ClearBit(AD0INTEN,8);
/* AD1 */
/* PCLK/4 ( 3.75MHz) - BURST - ON */
AD1CR = 0x03 << 8 | 1 << 16 | 1 << 21;
/* disable global interrupt */
ClearBit(AD1INTEN,8);
/* select P0.29 as AD0.2 for bat meas*/
PINSEL1 |= 0x01 << 26;
/* sample AD0.2 */
AD0CR |= 1 << 2;
/* select P0.10 as AD1.2 for baro*/
ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT;
/* sample AD1.2 */
AD1CR |= 1 << 2;
/* turn on DAC pins */
PINSEL1 |= 2 << 18;
#ifdef USE_ADC_1
/* select P0.13 (ADC_SPARE) as AD1.4 adc 1 */
PINSEL0 |= 0x03 << 26;
AD1CR |= 1 << 4;
#endif
#ifdef USE_ADC_2
/* select P0.4 (SCK_0) as AD0.6 adc 2 */
PINSEL0 |= 0x03 << 8;
AD0CR |= 1 << 6;
#endif
#ifdef USE_ADC_3
/* select P0.5 (MISO_0) as AD0.7 adc 3 */
PINSEL0 |= 0x03 << 10;
AD0CR |= 1 << 7;
#endif
#ifdef USE_ADC_4
/* select P0.6 (MOSI_0) as AD1.0 adc 4 */
PINSEL0 |= 0x03 << 12;
AD1CR |= 1 << 0;
#endif
booz2_adc_1 = 0;
booz2_adc_2 = 0;
booz2_adc_3 = 0;
booz2_adc_4 = 0;
}
void InitKeyboard(void){
MenuIni();
ClearBit(KEY_PORTIN_OK, KEY_OK_PORT);
ClearBit(KEY_PORTIN_STEP, KEY_STEP_PORT);
#ifdef EXTERNAL_INTERAPTS_USE
MCUCR |= (1<<ISC01) | (1<<ISC00) | //По спадающему уровню на INTo
(1<<ISC11) | (1<<ISC10); //по спадающему на INT1
GICR |= (1<<INT0) | (1<<INT1); //Разрешить прерывание от кнопок
#else
SetTimerTask(KeyScan, SCAN_PERIOD_KEY);
#endif
PrevKey = 0;
Key = 0;
}
//общая функция
inline void LCDG_CommonFunc(unsigned char data)
{
ClearBit(PORT_LCD_CON, RD_WR);
asm("nop");
asm("nop");
ClearBit(PORT_LCD_EN, EN);
PORT_LCD_BUS = data;
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
SetBit(PORT_LCD_EN, EN);
}
//==================================================================================
void SetBoxPos(U8 _box_num, U16 _boxhl, U16 _boxvt)
{
U8 _rdat_;
_rdat_ = ReadAsicByte(MASTER,DVC_PG2,0x21+_box_num*6);
if(BitSet(_boxhl, BIT0)) SetBit(_rdat_, BIT3);
else ClearBit(_rdat_, BIT3);
if(BitSet(_boxvt, BIT0)) SetBit(_rdat_, BIT1);
else ClearBit(_rdat_, BIT1);
WriteAsicByte(MASTER,DVC_PG2,0x21+_box_num*6,_rdat_);
WriteAsicByte(MASTER,DVC_PG2,0x22+_box_num*6,(U8)(_boxhl>>1));
WriteAsicByte(MASTER,DVC_PG2,0x24+_box_num*6,(U8)(_boxvt>>1));
}
void ServiceKonnectPWM(void)
{
static int FirstTime=TRUE;
static float Vc=0.0f;
static double T0;
static int State;
double T=Time_sec();
if (FirstTime)
{
FirstTime=FALSE;
T0=T;
State=0;
}
else
{
float V,I;
float RPM=*(float *)&persist.UserData[KMVAR];
float Vout = RPM*(5.0 / 1000.0);
// Compute Voltage applied to Cap
V=Vcc*State;
// Compute current
I=(V-Vc)/R;
// Compute new Cap Voltage
Vc += I/C*(T-T0);
// determine next state
if (Vc > Vout)
{
ClearBit(HIGH_BIT);
SetBit(LOW_BIT);
State=0;
}
else
{
ClearBit(LOW_BIT);
SetBit(HIGH_BIT);
State=1;
}
T0=T; // save time when applied
}
}
void LCDG_SendSymbol(unsigned char xPos, unsigned char yPos,unsigned char data)
{
unsigned char i;
unsigned char realAdr;
unsigned char realPage = PAGE0 + yPos;
unsigned int pointerFont = ((unsigned int)data<<2) + ((unsigned int)data<<1);
realAdr = PlaceArray[xPos];
xPos = realAdr;
for(i = 0; i<=5; i++){
if (realAdr < 61) {
SetBit(PORT_LCD_CON, CS);
realAdr = realAdr + 19;
}
else {
ClearBit(PORT_LCD_CON, CS);
realAdr = realAdr - 61;
}
data = pgm_read_byte(&(Font_6x8_Data[pointerFont]));
if(flag) data = ~data;
LCDG_SendCom(realPage);
LCDG_SendCom(realAdr);
LCDG_SendData(data);
xPos++;
realAdr = xPos;
pointerFont++;
}
}
//отображает один пиксел на экране
void LCDG_PutPixel(unsigned char xPos, unsigned char yPos)
{
unsigned char realAdr;
unsigned char realPage;
unsigned char data;
unsigned char currentData;
if ((xPos > 122) || (yPos > 31)) return;
realAdr = xPos;
realPage = PAGE0 + (yPos/8);
data = yPos%8;
if (realAdr < 61) {
SetBit(PORT_LCD_CON, CS);
realAdr = realAdr + 19;
}
else {
ClearBit(PORT_LCD_CON, CS);
realAdr = realAdr - 61;
}
LCDG_WaitFLAG_BUSY();
LCDG_SendCom(realPage);
LCDG_SendCom(realAdr);
LCDG_ReadData();
currentData = LCDG_ReadData();
switch(method){
case MET_AND: {currentData &= ~(1<<data); break;}
case MET_OR : {currentData |= (1<<data); break;}
case MET_XOR : {currentData ^= (1<<data); break;}
}
LCDG_SendCom(realAdr);
LCDG_SendData(currentData);
}
/* 1 - 7 is the range we use for the global modifiers array
* above */
assert(mods->modifiers > 0);
assert(mods->modifiers <= 7);
assert(mods->modifiers % 2 == 1); /* because we fail odd ones */
assert(mods->status != Success);
assert(mods->pad0 == 0);
assert(mods->pad1 == 0);
}
reply_handler = reply_XIPassiveGrabDevice;
}
static void request_XIPassiveGrabDevice(ClientPtr client, xXIPassiveGrabDeviceReq* req, int error, int errval)
{
int rc;
int modifiers;
rc = ProcXIPassiveGrabDevice(&client_request);
assert(rc == error);
if (rc != Success)
assert(client_request.errorValue == errval);
client_request.swapped = TRUE;
swaps(&req->length);
swapl(&req->time);
swapl(&req->grab_window);
swapl(&req->cursor);
swapl(&req->detail);
swaps(&req->deviceid);
modifiers = req->num_modifiers;
swaps(&req->num_modifiers);
swaps(&req->mask_len);
while(modifiers--)
{
CARD32 *mod = ((CARD32*)(req + 1)) + modifiers;
swapl(mod);
}
rc = SProcXIPassiveGrabDevice(&client_request);
assert(rc == error);
if (rc != Success)
assert(client_request.errorValue == errval);
}
static unsigned char *data[4096]; /* the request buffer */
static void test_XIPassiveGrabDevice(void)
{
int i;
xXIPassiveGrabDeviceReq *request = (xXIPassiveGrabDeviceReq*)data;
unsigned char *mask;
request_init(request, XIPassiveGrabDevice);
request->grab_window = CLIENT_WINDOW_ID;
reply_handler = reply_XIPassiveGrabDevice;
client_request = init_client(request->length, request);
printf("Testing invalid device\n");
request->deviceid = 12;
request_XIPassiveGrabDevice(&client_request, request, BadDevice, request->deviceid);
request->deviceid = XIAllMasterDevices;
printf("Testing invalid grab types\n");
for (i = XIGrabtypeFocusIn + 1; i < 0xFF; i++)
{
request->grab_type = i;
request_XIPassiveGrabDevice(&client_request, request, BadValue, request->grab_type);
}
printf("Testing invalid grab type + detail combinations\n");
request->grab_type = XIGrabtypeEnter;
request->detail = 1;
request_XIPassiveGrabDevice(&client_request, request, BadValue, request->detail);
request->grab_type = XIGrabtypeFocusIn;
request_XIPassiveGrabDevice(&client_request, request, BadValue, request->detail);
request->detail = 0;
printf("Testing invalid masks\n");
mask = (unsigned char*)&request[1];
request->mask_len = bytes_to_int32(XI2LASTEVENT + 1);
request->length += request->mask_len;
SetBit(mask, XI2LASTEVENT + 1);
request_XIPassiveGrabDevice(&client_request, request, BadValue, XI2LASTEVENT + 1);
ClearBit(mask, XI2LASTEVENT + 1);
/* tested all special cases now, test a few valid cases */
/* no modifiers */
request->deviceid = XIAllDevices;
request->grab_type = XIGrabtypeButton;
request->detail = XIAnyButton;
request_XIPassiveGrabDevice(&client_request, request, Success, 0);
/* some modifiers */
request->num_modifiers = N_MODS;
request->length += N_MODS;
memcpy((uint32_t*)(request + 1) + request->mask_len, modifiers, sizeof(modifiers));
request_XIPassiveGrabDevice(&client_request, request, Success, 0);
}
void RC522ClearBit(uint8 Address,uint8 loc)
{
uint8 temp;
temp=RC522Read(Address); //读取原有数据
temp=ClearBit(temp,loc); //处理数据
RC522Write(Address,temp); //写入处理完成后的数据
}
void imu_aspirin_arch_init(void)
{
/* set input pin for gyro eoc */
ClearBit(ASPIRIN_GYRO_EOC_IODIR, ASPIRIN_GYRO_EOC_PIN);
//ASPIRIN_GYRO_EOC_PINSEL |= ASPIRIN_GYRO_EOC_PINSEL_VAL << ASPIRIN_GYRO_EOC_PINSEL_BIT;
}
void LCD_WriteComInit(unsigned char data)
{
_delay_us(40);
ClearBit(PORT_SIG_RS, RS); //установка RS в 0 - команды
#ifdef BUS_4BIT
unsigned char tmp;
tmp = PORT_DATA & 0x0F;
tmp |= (data & 0xF0);
PORT_DATA = tmp; //вывод старшей тетрады
#else
PORT_DATA = data; //вывод данных на шину индикатора
#endif
SetBit(PORT_SIG_EN, EN); //установка E в 1
_delay_us(LCD_SPEED);
ClearBit(PORT_SIG_EN, EN); //установка E в 0 - записывающий фронт
}
int main()
{
int BITFIELDLENGTH = 512; //16*32 is 512
// bitfieldlength is evenly divisible by 32, right? right?
unsigned long size = (BITFIELDLENGTH/(sizeof(int)*8));
printf("sizeof int is %lu bytes (%lu bits)\n",sizeof(int), sizeof(int)*8);
printf("field length %d means %lu integers and %lu bytes\n",
BITFIELDLENGTH, size, (size*sizeof(int)));
//allocate and initialize to 0
int *BitField = calloc(size,sizeof(int));
SetBit(BitField,99); //set bit 99 to 1
SetBit(BitField,100); //set bit 100 to 1
SetBit(BitField,101); //set bit 101 to 1
int a = TestBit(BitField,99); //get val of bits 99-101
int b = TestBit(BitField,100);
int c = TestBit(BitField,101);
printf("vals are: %d, %d, %d\n",a,b,c);
ClearBit(BitField,100); //clear bit 100
a = TestBit(BitField,99);
b = TestBit(BitField,100);
c = TestBit(BitField,101);
printf("vals are now: %d, %d, %d\n",a,b,c);
}
//функция очистки lcd
void LCDG_ClrDisp(unsigned char x1, unsigned char x2)
{
unsigned char xPos = x1;
unsigned char realAdr = 0;
while(xPos < x2){
if (realAdr < 61) {
SetBit(PORT_LCD_CON, CS);
realAdr = realAdr + 19;
}
else {
ClearBit(PORT_LCD_CON, CS);
realAdr = realAdr - 61;
}
LCDG_SendCom(PAGE0);
LCDG_SendCom(realAdr);
LCDG_SendData(0);
LCDG_SendCom(PAGE1);
LCDG_SendCom(realAdr);
LCDG_SendData(0);
LCDG_SendCom(PAGE2);
LCDG_SendCom(realAdr);
LCDG_SendData(0);
LCDG_SendCom(PAGE3);
LCDG_SendCom(realAdr);
LCDG_SendData(0);
xPos++;
realAdr = xPos;
}
}
/*
Volume_Clock routine
This routine pulses the clock line.
*/
void Volume_Clock(void)
{
SetBit(VolPort, VolClk);
NOP;
ClearBit(VolPort, VolClk);
NOP;
}
void ADCSelectChannel(char channel) {
//Seteamos el pin de ADC que se va a usar, como entrada
ClearBit(PORT_LDR, LDR_NUMBER);
ADMUX = ((ADMUX & 0xE0) | (0x07 & channel));
}
void vec_GF2::put(long i, GF2 a)
{
if (a == 1)
SetBit(*this, i);
else
ClearBit(*this, i);
}
main()
{
SetBitDirection(26,1); // Set bit 26 (PWM 0 as an output)
SetBitDirection(29,1); // Set bit 29 (DIR 0 as an output)
FPGA(IO_PWMS_PRESCALE) = 1; // set pwm period to 30 KHz
FPGA(IO_PWMS+1) = 1; // enable the PWM0
for (;;) //loop forever
{
WaitNextTimeSlice();
if (ch0->Enable)
{
if (ch0->Output >= 0.0f)
{
SetBit(29); // Direction
FPGA(IO_PWMS+0) = ch0->Output; // Magnitude
}
else
{
ClearBit(29); // Direction
FPGA(IO_PWMS+0) = -ch0->Output; // Magnitude
}
}
else
{
FPGA(IO_PWMS+0) = 0; // whenever not enabled put 0% duty cycle
}
}
}
//==================================================================================
void SetBoxSize(U8 _box_num, U16 _boxhw, U16 _boxvw)
{
// WriteAsicByte(MASTER,DVC_PG2,REG_BOXHW+5*_box_num,_boxhw);
// WriteAsicByte(MASTER,DVC_PG2,REG_BOXVW+5*_box_num,_boxvw);
U8 _rdat_;
_rdat_ = ReadAsicByte(MASTER,DVC_PG2,0x21+_box_num*6);
if(BitSet(_boxhw, BIT0)) SetBit(_rdat_, BIT2);
else ClearBit(_rdat_, BIT2);
if(BitSet(_boxvw, BIT0)) SetBit(_rdat_, BIT0);
else ClearBit(_rdat_, BIT0);
WriteAsicByte(MASTER,DVC_PG2,0x21+_box_num*6,_rdat_);
WriteAsicByte(MASTER,DVC_PG2,0x23+_box_num*6,(U8)(_boxhw>>1));
WriteAsicByte(MASTER,DVC_PG2,0x25+_box_num*6,(U8)(_boxvw>>1));
}
main()
{
int i,l;
double k=0;
volatile double n;
for(l=0;l<30;l++)
{
SetBit(47);
SetBit(46);
for (i=0;i<2000000;i++) k+=i;
n+=k;
ClearBit(47);
ClearBit(46);
for (i=0;i<2000000;i++) k+=i;;
n+=k;
}
}
void Bert::StopRxBertTest()
{
ClearBit(m_pcpr, 4);
m_board.Write(L_PCPR, 0x11);
ClearChannelsMask();
m_board.Write(L_PCPR, m_pcpr);
SetChannelsMask();
byte temp = m_board.Read(L_PCPR);
if(!TestBit(temp, 0x0)) m_board.MaskClear(L_BIC, 0x01);
}
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_i = *enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
waypoint_globalize(wp_id);
}
}
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_f = *enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
waypoint_globalize(wp_id);
}
}
