/*
连续读出ADXL345内部加速度数据，地址范围0x32~0x37
*/
void Multiple_read(short *x,short *y,short *z)
{
unsigned char i;
start_i2c(); //起始信号
sendbyte(ADXL_WRITE); //发送设备地址+写信号
i2c_Wait_Ack();
sendbyte(0x32);
i2c_Wait_Ack();
start_i2c(); //起始信号
sendbyte(ADXL_READ); //发送设备地址+读信号
i2c_Wait_Ack();
for (i=0; i<6; i++) //连续读取6个地址数据，存储中BUF
{
ADXL_BUF[i] = rcvbyte(); //BUF[0]存储0x32地址中的数据
if (i == 5)
{
send_ack(1); //最后一个数据需要回NOACK
}
else
{
send_ack(0); //回应ACK
}
}
stop_i2c( ); //停止信号
*x=(short)(((u16)ADXL_BUF[1]<<8)+ADXL_BUF[0]);
*y=(short)(((u16)ADXL_BUF[3]<<8)+ADXL_BUF[2]);
*z=(short)(((u16)ADXL_BUF[5]<<8)+ADXL_BUF[4]);
}
static void printNibble(unsigned char nibble) {
if (nibble < 10) {
sendbyte('0' + nibble);
} else {
sendbyte('A' + (nibble - 10));
}
}
static void recalibrate(void) /* recalibrate the drive */
{
/* send actual command bytes */
sendbyte(CMD_RECAL);
sendbyte(0);
/* wait until seek finished */
waitfdc(1);
}
void Single_Write(unsigned char REG_Address,unsigned char REG_data)
{
start_i2c(); //起始信号
sendbyte(ADXL_WRITE); //发送设备地址+写信号
i2c_Wait_Ack();
sendbyte(REG_Address); //内部寄存器地址
i2c_Wait_Ack();
sendbyte(REG_data); //内部寄存器数据
i2c_Wait_Ack();
stop_i2c(); //发送停止信号+-+/
}
int Single_Write_BH1750(u8 REG_Address)
{
start_i2c(); //起始信号
sendbyte(SlaveAddress); //发送设备地址+写信号
i2c_Wait_Ack();
sendbyte(REG_Address); //内部寄存器地址，
i2c_Wait_Ack();
stop_i2c(); //发送停止信号
return 0;
}
/* this gets the FDC to a known state */
static void reset(void)
{
outportb(FDC_DOR,0);
mtick=0;
motor=0;
outportb(FDC_DRS,0);
outportb(FDC_DOR,0x0c);
done=1;
sendbyte(CMD_SPECIFY);
sendbyte(0xdf); /* SRT = 3ms, HUT = 240ms */
sendbyte(0x02); /* HLT = 16ms, ND = 0 */
flseek(1);
recalibrate();
dchange = 0;
}
/* seek to track */
static UINT flseek(int track)
{
if (fdc_track==track) return 1; /* already there? */
if(!motor) motoron();
/* send actual command bytes */
sendbyte(CMD_SEEK);
sendbyte(0);
sendbyte(track);
/* wait until seek finished */
if (!waitfdc(1)) return 0; /* timeout! */
/* now let head settle for 15ms */
sleep(15);
/* check that seek worked */
return !((sr0!=0x20) || (fdc_track!=track));
}
void pt6302_sendcmd(uint8 cmd, uint8* buf, uint8 len)
{
uint8 i;
pin_csb = HI_LEVEL;
delay(1);
pin_csb = LOW_LEVEL;
sendbyte(cmd);
for(i = 0; i < len; i++)
{
delay(TDOFF);
sendbyte(buf[i]);
}
delay(TCSH);
pin_csb = HI_LEVEL;
delay(1);
}
void Adafruit_SharpMem::toggleVcom(void)
{
if (_clk == -1)
SPI.beginTransaction(sharpmemSPI);
SPI_CS_ENABLE();
delayMicroseconds(SPI_DEFAULT_DELAY_US);
sendbyte(SHARPMEM_BIT_WRITECMD | _sharpmem_vcom);
TOGGLE_VCOM;
sendbyte(SHARPMEM_BIT_DUMMY);
delayMicroseconds(SPI_DEFAULT_DELAY_US);
SPI_CS_DISABLE();
if (_clk == -1)
SPI.endTransaction();
}
u8 Multiple_read_BH1750(void)
{ u8 i;
start_i2c(); //起始信号
sendbyte(SlaveAddress+1); //发送设备地址+读信号
i2c_Wait_Ack();
for (i=0; i<3; i++) //连续读取2个地址数据，存储中BUF
{
BUF[i] = rcvbyte(); //BUF[0]存储0x32地址中的数据
if (i == 3)
{
send_ack(1); //最后一个数据需要回NOACK
}
else
{
send_ack(0); //回应ACK
}
}
stop_i2c(); //停止信号
delay_ms(5);
return 1;
}
int cafiine_fread(int sock, int *result, void *ptr, int size, int count, int fd) {
while (bss.lock) GX2WaitForVsync();
bss.lock = 1;
CHECK_ERROR(sock == -1);
int ret;
char buffer[1 + 12];
buffer[0] = BYTE_READ;
*(int *)(buffer + 1) = size;
*(int *)(buffer + 5) = count;
*(int *)(buffer + 9) = fd;
ret = sendwait(sock, buffer, 1 + 12);
CHECK_ERROR(ret < 0);
ret = recvbyte(sock);
CHECK_ERROR(ret < 0);
CHECK_ERROR(ret == BYTE_NORMAL);
ret = recvwait(sock, result, 4);
CHECK_ERROR(ret < 0);
int sz;
ret = recvwait(sock, &sz, 4);
CHECK_ERROR(ret < 0);
ret = recvwait(sock, ptr, sz);
CHECK_ERROR(ret < 0);
ret = sendbyte(sock, BYTE_OK);
CHECK_ERROR(ret < 0);
bss.lock = 0;
return 0;
error:
bss.lock = 0;
return -1;
}
unsigned char Single_Read(unsigned char REG_Address)
{
unsigned char REG_data;
start_i2c(); //起始信号/
sendbyte(ADXL_WRITE); //发送设备地址+写信号
i2c_Wait_Ack();
sendbyte(REG_Address); //发送存储单元地址，从0开始
i2c_Wait_Ack();
start_i2c(); //起始信号
sendbyte(ADXL_READ); //发送设备地址+读信号
i2c_Wait_Ack();
REG_data=rcvbyte(); //读出寄存器数据
send_ack(1);
stop_i2c( ); //停止信号
return REG_data;
}
void sendstr(unsigned char *s)
{
while(*s!='\0')
{
sendbyte(*s);
s++;
}
}
void write(unsigned char start, unsigned char ddata) //写指令或数据
{
unsigned char start_data,Hdata,Ldata;
if(start == COMMAND)
start_data = 0xf8; //写指令
else
start_data = 0xfa; //写数据
Hdata = ddata&0xf0; //取高四位
Ldata = (ddata<<4)&0xf0; //取低四位
sendbyte(start_data); //发送起始信号
clock_delay_usec(100); //延时是必须的
sendbyte(Hdata); //发送高四位
clock_delay_usec(100); //延时是必须的
sendbyte(Ldata); //发送低四位
clock_delay_usec(100); //延时是必须的
}
void Adafruit_SharpMem::clearDisplay(void)
{
memset(sharpmem_buffer, 0xff, (SHARPMEM_LCDWIDTH * SHARPMEM_LCDHEIGHT) / 8);
if (_clk == -1)
SPI.beginTransaction(sharpmemSPI);
// Send the clear screen command rather than doing a HW refresh (quicker)
SPI_CS_ENABLE();
delayMicroseconds(SPI_DEFAULT_DELAY_US);
sendbyte(_sharpmem_vcom | SHARPMEM_BIT_CLEAR);
sendbyte(SHARPMEM_BIT_DUMMY);
TOGGLE_VCOM;
delayMicroseconds(SPI_DEFAULT_DELAY_US);
SPI_CS_DISABLE();
if (_clk == -1)
SPI.endTransaction();
}
/*
* Send a set of bytes to the chip. We need to pulse the MODE line
* between each byte, but never at the start nor at the end of the
* transfer.
*/
static void sendbytes(struct l3_pins *adap, const u8 *buf,
int len)
{
int i;
for (i = 0; i < len; i++) {
if (i) {
udelay(adap->mode_hold);
adap->setmode(0);
udelay(adap->mode);
}
adap->setmode(1);
udelay(adap->mode_setup);
sendbyte(adap, buf[i]);
}
}
void WriteCommand(uchar Command) //send command
{
lcd_wr = 1; // init all control signal
lcd_rd = 1;
lcd_a0 = 1; // for command
//P2 = Command;
sendbyte(Command) ;
lcd_cs = 0; // enable the access
_nop_();
lcd_wr = 0;
_nop_();
_nop_();
lcd_wr = 1;
_nop_();
lcd_cs = 1; // disable the access
delayms(1);
}
void WriteData(uchar DData) //send display data
{
lcd_wr = 1; // init all control signal
lcd_rd = 1;
lcd_a0 = 0; // for diaplay data
//P2 = DData;
sendbyte(DData);
lcd_cs = 0; // enable the access
_nop_();
lcd_wr = 0;
_nop_();
_nop_();
lcd_wr = 1;
_nop_();
lcd_cs = 1; // disable the access
_nop_();
_nop_();
}
int l3_write(struct l3_pins *adap, u8 addr, u8 *data, int len)
{
adap->setclk(1);
adap->setdat(1);
adap->setmode(1);
udelay(adap->mode);
adap->setmode(0);
udelay(adap->mode_setup);
sendbyte(adap, addr);
udelay(adap->mode_hold);
sendbytes(adap, data, len);
adap->setclk(1);
adap->setdat(1);
adap->setmode(0);
return len;
}
/* this waits for FDC command to complete */
static UINT waitfdc(UINT sensei)
{
tmout = 1000; /* set timeout to 1 second */
/* wait for IRQ6 handler to signal command finished */
thepid=sys_getpid();
while (!done && tmout) sys_pause();
/* read in command result bytes */
statsz = 0;
while ((statsz<7) && (inportb(FDC_MSR)&(1<<4))) {
status[statsz++] = getbyte();
}
if (sensei) { /* send a "sense interrupt status" command */
sendbyte(CMD_SENSEI);
sr0=getbyte();
fdc_track=getbyte();
}
if (!tmout) { /* timed out! */
if (inportb(FDC_DIR) & 0x80) /* check for diskchange */
dchange=1;
return 0;
} else return 1;
}
void Adafruit_SharpMem::refresh(void)
{
uint16_t i, totalbytes, currentline, oldline;
totalbytes = (SHARPMEM_LCDWIDTH * SHARPMEM_LCDHEIGHT) / 8;
if (_clk == -1)
SPI.beginTransaction(sharpmemSPI);
// Send the write command
SPI_CS_ENABLE();
delayMicroseconds(SPI_DEFAULT_DELAY_US);
sendbyte(SHARPMEM_BIT_WRITECMD | _sharpmem_vcom);
TOGGLE_VCOM;
// Send the address for line 1
oldline = currentline = 1;
sendbyte(currentline);
// Send image buffer
for (i=0; i<totalbytes; i++)
{
sendbyte(sharpmem_buffer[i]);
currentline = ((i+1)/(SHARPMEM_LCDWIDTH/8)) + 1;
if(currentline != oldline)
{
// Send end of line and address bytes
sendbyte(SHARPMEM_BIT_DUMMY);
if (currentline <= SHARPMEM_LCDHEIGHT)
{
sendbyte(currentline);
}
oldline = currentline;
}
}
// Send another trailing 8 bits for the last line
sendbyte(SHARPMEM_BIT_DUMMY);
delayMicroseconds(SPI_DEFAULT_DELAY_US);
SPI_CS_DISABLE();
if (_clk == -1)
SPI.endTransaction();
}
void closeStdout(void)
{
sendbyte(-1);
}
int main(void){
int integration[3] = {0,0,0};
char lostsignalcnt = 0;
int pry[] = {0,0,0};
int paceCounter = 0;
int pidValues13[3] = {6,20,24};
int pidValuesDen13[3] = {16,1,1};
int pidValues24[3] = {6,20,24};
int pidValuesDen24[3] = {16,1,1};
char pidRotUp[3] = {0,0,20};
char pidRotDenUp[3] = {42,1,1};
char pidRotDown[3] = {9,0,20};
char pidRotDenDown[3] = {42,1,1};
char pidRot[] = {5,0,20};
int throttledif = 0;
int throttleavr = 0;
/*counting var, for for loops*/
int i;
/*Start memory location for Accel and Gyro reads, should be moved
to gyro and accel read functions*/
uint8_t accelstartbyte = 0x30;
uint8_t gyrostartbyte = 0x1A;
/*Joystick Axis buffer
[0] - X axis tilt
[1] - Y axis tilt
[2] - Throttle
[3] - Rotation about Z axis
*/
int joyaxis[] = {0,0,0,0,0};
char joyin[] = {0,0,0,0,0};
int joytrim[] = {0,0,0,0,0};
int joydif[] = {0,0};
int joyavr[] = {0,0};
int motorSpeeds[4];
/*Var to allow increase in motor speed nonrelative to the throttle
during flight*/
int motorup = 0;
/*Vars for new input raw data (cache) and filtered data (int) from
imu*/
int gyrocache[3] = {0,0,0};
int accelcache[3] = {0,0,0};
int magcache[3] = {0,0,0};
int magfacing = 0;
int roterr = 0;
int target[] = {0,0,0};
int accelint[] = {0, 0, 0};
int gyroint[] = {0, 0, 0};
int gyrocounter[] = {0,0,0};
/*Standard values for accel and gyro (when level), set during offset*/
int accelnorm[3] = {28,-20,468};
char gyronorm[3] = {16,42,0};
/*Buffer for sending data through the xbee*/
char xbeebuffer[100];
CLK.CTRL = 0b00000011;
CLK.PSCTRL = 0b00010100;
/*Initialize PORTD to output on pins 0-3 from Timer counter pwm at
50Hz*/
PORTD.DIR = 0x2F;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCCEN_bm | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCDEN_bm;
TCD0.PER = 8000;
/*Initialize Timer counter C0 for pacing,RATE Hz*/
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 2000000 / RATE;
/*Set on board LED pins to output*/
PORTF.DIR = 0x03;
/*Set PORTC to power IMU, PIN 3 3.3V, pin 2 ground*/
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
/*Enable global interrupts of all priority levels, should be made
more relevant*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/*Set pwm duty cycle to stop motors, stop them from beeping
annoyingly*/
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
/*Set Xbee Uart transmit pin 3 to output*/
PORTE.DIR = 0x08;
/*Initialize USARTE0 as the module used by the Xbee*/
uartInitiate(&xbee, &USARTE0);
/*Initialize USARTE1 as the module used by atmega328p */
uartInitiate(&atmega328p, &USARTE1);
/*Initialize imu to use Two wire interface on portC*/
twiInitiate(&imu, &TWIC);
itg3200Init(&imu, RATE);
adxl345Init(&imu);
lsm303dlhInit(&imu);
/*Send string to indicate startup, python doesn't like return carriage
(/r) character*/
sprintf(xbeebuffer, "starting\n");
sendstring(&xbee, xbeebuffer);
/*Start of flight control state machine loop*/
while(1){
/*Check for new packet from atmega328p*/
if(IRDataAvailable)
{
sprintf(xbeebuffer, "Altitude: %d\n", (irdata[1]*256) + irdata[2]);
sendstring(&xbee, xbeebuffer);
}
/*Check for new packet from xbee each time*/
if(readdata){
readdata = 0;
lostsignalcnt = 0;
/*For Joystick packet reads*/
joytrim[2] = 0;
for(i = 0; i < 5; i++){
joyin[i] = -input[3 + i] + 126;
joyaxis[i] = joyin[i];
joyaxis[i] += joytrim[i];
}
throttleavr = ((throttleavr) + (joyaxis[2]))/2;
throttledif = joyaxis[2] - throttleavr;
joyaxis[2] += throttledif * THROTTLEJOYDIF;
for(i = 0; i < 2; i++){
joyavr[i] = (joyavr[i] + joyaxis[i])/2;
joydif[i] = joyaxis[i] - joyavr[i];
}
joyaxis[1] += joydif[1] * PRJOYDIF13;
joyaxis[0] += joydif[0] * PRJOYDIF24;
/*
yawavr = ((yawavr) + joyaxis[3])/2;
yawdif = joyaxis[3] - yawavr;
joyaxis[3] += yawdif * YAWJOYDIF;
*/
//Input 7 is the button buffer
if(input[8] == 4){
state = stopped;
//sprintf(xbeebuffer, "stopped %d\n", input[7]);
sprintf(xbeebuffer, "%4d %4d %4d %4d\n", joyaxis[0], joyaxis[1], joyaxis[2], joyaxis[3]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 0){
joytrim[0] += joyin[0];
joytrim[1] += joyin[1];
joytrim[3] += joyin[3];
}
else if(input[8] == 1){
state = running;
sprintf(xbeebuffer, "running %d\n", input[7]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 10){
state = offset;
}
else if(input[8] == 5){
//motorup += 5;
pidRot[2] ++;
sprintf(xbeebuffer, "D up %d\n", pidRot[2]);
//pidRot[2] ++;
//sprintf(xbeebuffer, "D up %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 6){
pidRot[2] --;
sprintf(xbeebuffer, "D down %d\n", pidRot[2]);
//pidRot[2] --;
//sprintf(xbeebuffer, "D down %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
//motorup -= 5;
}
else if(input[8] == 7){
pidRot[0] ++;
sprintf(xbeebuffer, "P up %d\n", pidRot[0]);
//pidRot[0] ++;
//sprintf(xbeebuffer, "P up %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 8){
pidRot[0] --;
sprintf(xbeebuffer, "P down %d\n", pidRot[0]);
//pidRot[0] --;
//sprintf(xbeebuffer, "P down %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
/*
getmag(magcache, &imu);
sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sendstring(&xbee, xbeebuffer);
*/
}
else if(input[8] == 2){
sprintf(xbeebuffer, "descending\n");
sendstring(&xbee, xbeebuffer);
motorup = -50;
}
xbeecounter = 0;
for(i=0;i<3;i++){
pidRotUp[i] = pidRot[i] * 3/4;
pidRotDown[i] = pidRot[i] * 1;
}
if(state == running){
//sprintf(xbeebuffer, "%d %d\n", joyaxis[2], throttledif);
//sprintf(xbeebuffer, "%d %d %d \n", joyaxis[0], joyaxis[1], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", pry[0], pry[1], pry[2]);
//sprintf(xbeebuffer, "%3d %3d\n", gyroint[2], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n", motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", accelint[0], accelint[1], accelint[2]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sprintf(xbeebuffer, "%4d\n", roterr);
sendstring(&xbee, xbeebuffer);
}
}
switch(state){
/*Stopped state keeps motors stopped but not beeping*/
case stopped:
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
break;
/*Offset gets standard value for gyro's and accel's*/
case offset:
getgyro(gyrocache, &imu, &gyrostartbyte);
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
target[2] = arctan2(magcache[0], magcache[1]);
for(i = 0; i < 3; i ++){
gyronorm[i] = gyrocache[i];
//accelnorm[i] = accelcache[i];
accelcache[i] = 0;
gyrocache[i] = 0;
}
sprintf(xbeebuffer, "offset %d %d %d %d %d %d\n", gyronorm[0], gyronorm[1], gyronorm[2], accelnorm[0], accelnorm[1], accelnorm[2]);
sendstring(&xbee, xbeebuffer);
state = stopped;
break;
case running:
/*Ensure loop doesn't go faster than 50Hz*/
while(!(TCC0.INTFLAGS & 0x01));
TCC0.INTFLAGS = 0x01;
/*Get gyro data
substract stationary offset
filter for stability
*/
getgyro(gyrocache, &imu, &gyrostartbyte);
for(i = 0; i < 3; i ++){
gyrocache[i] -= gyronorm[i];
if((gyrocache[i] <= 1) && (gyrocache[i] >= -1)){
gyrocache[i] = 0;
}
gyrocounter[i] += gyrocache[i];
}
for(i = 0; i < 3; i += 2){
gyroint[i] = gyrocounter[i]/DEGREE;
gyrocounter[i] %= DEGREE;
pry[i] += gyroint[i];
if(pry[i] > PI){
pry[i] = -PI + (pry[i] - PI);
}
else if(pry[i] < -PI){
pry[i] = PI + (pry[i] + PI);
}
}
gyroint[1] = -(gyrocounter[1]/DEGREE);
gyrocounter[1] %= DEGREE;
pry[1] += gyroint[1];
paceCounter ++;
//Slower Operations at 50Hz
if(paceCounter == (RATE / 20)){
paceCounter = 0;
lostsignalcnt ++;
sendbyte(&atmega328p, 'r'); //ask for IR data
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
magfacing = arctan2(magcache[0], magcache[1]);
if((4900 - abs(pry[2]) - abs(target[2])) < abs(pry[2] - target[2])){
if(target > 0){
roterr = 4900 - abs(target[2]) - abs(pry[2]);
}
else{
roterr = -(4900 - abs(target[2]) - abs(pry[2]));
}
}
else{
roterr = target[2] - pry[2];
}
for(i = 0; i < 3; i ++){
accelcache[i] -= accelnorm[i];
/*
if(accelcache[i] > (accelint[i] + 40)){
accelcache[i] = accelint[i] + 40;
}
else if(accelcache[i] < (accelint[i] - 40)){
accelcache[i] = accelint[i] - 40;
}
*/
}
accelint[0] = ((ACCELINT * accelint[0]) + ((24 - ACCELINT) * accelcache[0]))/24;
accelint[1] = ((ACCELINT * accelint[1]) + ((24 - ACCELINT) * accelcache[1]))/24;
if(accelint[1] > (pry[0] + 15)){
accelint[1] = pry[0] + 15;
}
else if(accelint[1] < (pry[0] - 15)){
accelint[1] = pry[0] - 15;
}
if(accelint[0] > (pry[1] + 15)){
accelint[0] = pry[1] + 15;
}
else if(accelint[0] < (pry[1] - 15)){
accelint[0] = pry[1] - 15;
}
pry[0] = ((AWEIGHT * accelint[1]) + (GWEIGHT * pry[0])) / (AWEIGHT + GWEIGHT);
pry[1] = ((AWEIGHT * accelint[0]) + (GWEIGHT * pry[1])) / (AWEIGHT + GWEIGHT);
/*reset cache values to 0, should be made unnecessary by modding gyro and
accel read functions*/
for(i = 0; i < 3; i ++){
accelcache[i] = 0;
}
}
if(gyroint[0] > 6){
gyroint[0] = 6;
}
else if(gyroint[0] < -6){
gyroint[0] = -6;
}
if(gyroint[1] > 6){
gyroint[1] = 6;
}
else if(gyroint[1] < -6){
gyroint[1] = -6;
}
motorSpeed(pry, integration ,gyroint, joyaxis, motorSpeeds, pidValues13, pidValues24, pidValuesDen13, pidValuesDen24);
yawCorrect(motorSpeeds, gyroint, &roterr,pidRotUp,pidRotDenUp,pidRotDown,pidRotDenDown);
if(lostsignalcnt > 10){
for(i = 0; i < 4; i ++){
motorSpeeds[i] -= 50;
}
}
while(!((TCD0.CNT > 5000) || (TCD0.CNT < 2500)));
TCD0.CCA = motorSpeeds[0] + motorup;// - motordif13;
TCD0.CCC = motorSpeeds[2] + motorup;// + motordif13;
TCD0.CCB = motorSpeeds[1] + motorup;// + motordif24;
TCD0.CCD = motorSpeeds[3] + motorup;// - motordif24;
PORTD.OUT ^= 0b00100000;
}
}
}
void printROMStr(const char* str) {
while (pgm_read_byte(str)) {
sendbyte(pgm_read_byte(str));
str++;
}
}
void printStr(const char* str) {
while (*str) {
sendbyte(*str);
str++;
}
}
/*since reads and writes differ only by a few lines, this handles both.*/
static UINT fdc_rw(int block,char *blockbuff,UINT cmd_read,ULONG nosectors)
{
int head,track,sector,tries, copycount = 0;
char *p_tbaddr = (char *)0x80000;
char *p_blockbuff = blockbuff;
block2hts(block,&head,&track,§or); /* convert logical address into physical address */
motoron(); /* spin up the disk */
if (!cmd_read && blockbuff) {
/* copy data from data buffer into track buffer */
for(copycount=0;copycount<(nosectors*FLOPPY_SECTOR_SIZE);copycount++) {
*p_tbaddr=*p_blockbuff;
p_blockbuff++;
p_tbaddr++;
}
}
for (tries=0;tries<3;tries++) {
if (inportb(FDC_DIR) & 0x80) { /* check for diskchange */
dchange=1;
flseek(1); /* clear "disk change" status */
recalibrate();
motoroff();
return fdc_rw(block, blockbuff, cmd_read, nosectors);
}
if (!flseek(track)) {/* move head to right track */
motoroff();
return 0;
}
outportb(FDC_CCR,0);/* program data rate (500K/s) */
if (cmd_read) { /* send command */
dma_xfer(2,tbaddr,nosectors*FLOPPY_SECTOR_SIZE,0);
sendbyte(CMD_READ);
} else {
dma_xfer(2,tbaddr,nosectors*FLOPPY_SECTOR_SIZE,1);
sendbyte(CMD_WRITE);
}
sendbyte(head<<2);
sendbyte(track);
sendbyte(head);
sendbyte(sector);
sendbyte(2); /* 512 bytes/sector */
sendbyte(geometry.spt);
if (geometry.spt == DG144_SPT) sendbyte(DG144_GAP3RW); /* gap 3 size for 1.44M read/write */
else sendbyte(DG168_GAP3RW); /* gap 3 size for 1.68M read/write */
sendbyte(0xFF); /* DTL = unused */
/* wait for command completion */
/* read/write don't need "sense interrupt status" */
if (!waitfdc(1)) {
reset();
return fdc_rw(block, blockbuff, cmd_read, nosectors);
}
if (!(status[0]&0xC0)) break; /* worked! outta here! */
recalibrate(); /* oops, try again... */
}
motoroff();
if (cmd_read && blockbuff) {
/* copy data from track buffer into data buffer */
p_blockbuff=blockbuff;
p_tbaddr = (char *) 0x80000;
for(copycount=0; copycount<(nosectors*FLOPPY_SECTOR_SIZE);copycount++) {
*p_blockbuff=*p_tbaddr;
p_blockbuff++;
p_tbaddr++;
}
}
return (tries!=3);
}
