void main()
{
initialize_ports();
for(;;) {
//
// update leds based on switches
//
update_ds1to4();
//
// buzz buzzer
//
if(sw(S1)) {
costate {
buzzon();
waitfor(DelayMs(10));
buzzoff();
waitfor(DelayMs(200));
}
} else {
buzzoff();
}
//
// flash leds
//
costate {
waitfor(DelayMs(500));
ledoff(DS5);
waitfor(DelayMs(1000));
ledon(DS5);
}
costate {
waitfor(DelayMs(1000));
ledoff(DS6);
waitfor(DelayMs(500));
ledon(DS6);
}
costate {
waitfor(DelayMs(250));
ledoff(DS7);
waitfor(DelayMs(1250));
ledon(DS7);
}
costate {
waitfor(DelayMs(750));
ledoff(DS8);
waitfor(DelayMs(900));
ledon(DS8);
}
}
///////////////////////////////////////////////////////////
// set DS1 and DS2 leds to stay on and off at intervals
///////////////////////////////////////////////////////////
cofunc flashled[2](int led, int ontime, int offtime)
{
ledon(led);
waitfor(DelayMs(ontime));
ledoff(led);
waitfor(DelayMs(offtime));
}
/* Blinks the ACT LED. WARNING: NEVER RETURNS (INFINITE LOOP)*/
void ledblink()
{
while(1)
{
ledon();
Wait(500000);
ledoff();
Wait(500000);
}
}
void ledflash( uint32_t period , int duty )
{
if ( gettime() % period > (period*duty)/16 )
{
ledon(LEDALL);
}
else
{
ledoff(LEDALL);
}
}
/* main loop */
int
main(void)
{
input_line = (char *)malloc(6 * sizeof(char));
led_setperm();
set_time = 200000;
while (strncmp(input_line, "quit", 4) != 0)
{
input_line = readline("Led Control> ");
/* I know instruction parsing is really lame :/ */
if (!strncmp(input_line, "help", 4))
help();
if (!strncmp(input_line, "ledon", 5))
ledon();
if (!strncmp(input_line, "ledoff", 6))
ledoff();
if (!strncmp(input_line, "settime", 7))
settime();
if (!strncmp(input_line, "volume", 6))
volume();
if (!strncmp(input_line, "bin", 3))
bin();
if (!strncmp(input_line, "slide", 5))
slide();
if (!strncmp(input_line, "blink", 5))
blink();
if (!strncmp(input_line, "bislide", 7))
bislide();
if (!strncmp(input_line, "biblink", 7))
biblink();
if (!strncmp(input_line, "grow", 4))
grow();
if (!strncmp(input_line, "center", 6))
center();
if (!strncmp(input_line, "side", 4))
side();
if (!strncmp(input_line, "biside", 6))
biside();
if (!strncmp(input_line, "demo", 4))
demo();
}
led_off_all();
exit(0);
}
void failloop(int val)
{
for (int i = 0; i <= 3; i++)
{
pwm_set(i, 0);
}
while (1)
{
for (int i = 0; i < val; i++)
{
ledon(255);
delay(200000);
ledoff(255);
delay(200000);
}
delay(800000);
}
}
void rider (int select,int kleur,int tijd) //Select : 1=aan-links,2=uit-links,3=aan-rechts,4=uit-rechts,5=loop-links,6=loop-rechts,7=rider-links,8=rider-rechts Kleur : 1=blauw,2=groen,3=cyaan,4=rood,5=paars,6=geel,7=wit
{
switch (select)
{
case 1 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd); break;
case 2 : ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break;
case 3 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd); break;
case 4 : ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break;
case 5 : ledon (1,kleur); waitms(tijd); ledoff(1,kleur); ledon (2,kleur); waitms(tijd); ledoff(2,kleur);
ledon (3,kleur); waitms(tijd); ledoff(3,kleur); ledon (4,kleur); waitms(tijd); ledoff(4,kleur); break;
case 6 : ledon (4,kleur); waitms(tijd); ledoff(4,kleur); ledon (3,kleur); waitms(tijd); ledoff(3,kleur);
ledon (2,kleur); waitms(tijd); ledoff(2,kleur); ledon (1,kleur); waitms(tijd); ledoff(1,kleur); break;
case 7 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd);
ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break;
case 8 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd);
ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break;
default: return; break;
}
}
/* ... the usual ... */
int
main(void)
{
presses = 0;
firstpress = 0;
lastpress = 0;
lockdown = LOCKED_BY_DEFAULT;
strobo = STROBE_BY_DEFAULT;
/* initialize hardware */
init();
utx('I');
/* main loop */
while(1) {
int event = waitevent();
switch(event) {
case EVENT_RING:
utx('R');
if(!lockdown) {
/* if this is the first press, remember the time and enable signals */
if(!presses) {
firstpress = gettick();
}
/* blink the led */
ledoff();
waitticks(BLINKTIME);
ledon();
/* if the number of presses reaches the limit, open the door */
presses++;
/* record time of last keypress */
lastpress = gettick();
/* if pressed often enough */
if(presses >= PRESSES) {
strobooff();
opendoor();
presses = 0;
firstpress = 0;
lastpress = 0;
}
}
break;
case EVENT_TIMEOUT:
utx('T');
strobooff();
/* reset the sequence detector */
presses = 0;
firstpress = 0;
lastpress = 0;
break;
case EVENT_STROBE:
if(strobo)
stroboon();
break;
case EVENT_CMD_OPEN:
opendoor();
break;
case EVENT_CMD_LOCK:
utx('L');
lockdown = TRUE;
break;
case EVENT_CMD_UNLOCK:
utx('U');
lockdown = FALSE;
break;
case EVENT_CMD_STROBO_ENABLE:
utx('S');
strobo = TRUE;
break;
case EVENT_CMD_STROBO_DISABLE:
utx('s');
strobooff();
strobo = FALSE;
break;
}
}
}
void gyro_cal(void)
{
int data[6];
unsigned long time = gettime();
unsigned long timestart = time;
unsigned long timemax = time;
unsigned long lastlooptime;
// 2 and 15 seconds
while ( time - timestart < 2e6 && time - timemax < 15e6 )
{
unsigned long looptime;
looptime = time - lastlooptime;
lastlooptime = time;
if ( looptime == 0 ) looptime = 1;
//softi2c_readdata( 0x68 , 67 , data, 6 );
i2c_readdata( 67 , data, 6 );
gyro[0] = (int16_t) ((data[2]<<8) + data[3]);
gyro[1] = (int16_t) ((data[0]<<8) + data[1]);
gyro[2] = (int16_t) ((data[4]<<8) + data[5]);
if ( (time - timestart)%200000 > 100000)
{
ledon(B00000101);
ledoff(B00001010);
}
else
{
ledon(B00001010);
ledoff(B00000101);
}
for ( int i = 0 ; i < 3 ; i++)
{
if ( fabs(gyro[i]) > 100 )
{
count = 0;
timestart = gettime();
}
else
{
lpf( &gyrocal[i] , gyro[i], lpfcalc( (float) looptime , 0.5 * 1e6) );
count++;
}
}
time = gettime();
}
if ( count < 100 )
{
for ( int i = 0 ; i < 3; i++)
{
gyrocal[i] = 0;
}
}
#ifdef SERIAL
printf("gyro calibration %f %f %f \n " , gyrocal[0] , gyrocal[1] , gyrocal[2]);
#endif
}
int main(void)
{
clk_init();
gpio_init();
#ifdef SERIAL
serial_init();
#endif
i2c_init();
spi_init();
pwm_init();
pwm_set(MOTOR_FL, 0); // FL
pwm_set(MOTOR_FR, 0);
pwm_set(MOTOR_BL, 0); // BL
pwm_set(MOTOR_BR, 0); // FR
time_init();
if (RCC_GetCK_SYSSource() == 8)
{
}
else
{
failloop(5);
}
sixaxis_init();
if (sixaxis_check())
{
#ifdef SERIAL
printf(" MPU found \n");
#endif
}
else
{
#ifdef SERIAL
printf("ERROR: MPU NOT FOUND \n");
#endif
failloop(4);
}
adc_init();
rx_init();
int count = 0;
vbattfilt = 0.0;
while (count < 64)
{
vbattfilt += adc_read(1);
count++;
}
// for randomising MAC adddress of ble app - this will make the int = raw float value
random_seed = *(int *)&vbattfilt ;
random_seed = random_seed&0xff;
vbattfilt = vbattfilt / 64;
#ifdef SERIAL
printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif
#ifdef STOP_LOWBATTERY
// infinite loop
if (vbattfilt < STOP_LOWBATTERY_TRESH)
failloop(2);
#endif
// loads acc calibration and gyro dafaults
loadcal();
gyro_cal();
rgb_init();
imu_init();
extern unsigned int liberror;
if (liberror)
{
#ifdef SERIAL
printf("ERROR: I2C \n");
#endif
failloop(7);
}
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
checkrx();
while (1)
{
// gettime() needs to be called at least once per second
maintime = gettime();
looptime = ((uint32_t) (maintime - lastlooptime));
if (looptime <= 0)
looptime = 1;
looptime = looptime * 1e-6f;
if (looptime > 0.02f) // max loop 20ms
{
failloop(3);
//endless loop
}
lastlooptime = maintime;
if (liberror > 20)
{
failloop(8);
// endless loop
}
sixaxis_read();
control();
// battery low logic
float hyst;
float battadc = adc_read(1);
vbatt = battadc;
// average of all 4 motor thrusts
// should be proportional with battery current
extern float thrsum; // from control.c
// filter motorpwm so it has the same delay as the filtered voltage
// ( or they can use a single filter)
lpf ( &thrfilt , thrsum , 0.9968f); // 0.5 sec at 1.6ms loop time
lpf ( &vbattfilt , battadc , 0.9968f);
#ifdef AUTO_VDROP_FACTOR
static float lastout[12];
static float lastin[12];
static float vcomp[12];
static float score[12];
static int current_index = 0;
int minindex = 0;
float min = score[0];
{
int i = current_index;
vcomp[i] = vbattfilt + (float) i *0.1f * thrfilt;
if ( lastin[i] < 0.1f ) lastin[i] = vcomp[i];
float temp;
// y(n) = x(n) - x(n-1) + R * y(n-1)
// out = in - lastin + coeff*lastout
// hpf
temp = vcomp[i] - lastin[i] + FILTERCALC( 1000*12 , 1000e3) *lastout[i];
lastin[i] = vcomp[i];
lastout[i] = temp;
lpf ( &score[i] , fabsf(temp) , FILTERCALC( 1000*12 , 10e6 ) );
}
current_index++;
if ( current_index >= 12 ) current_index = 0;
for ( int i = 0 ; i < 12; i++ )
{
if ( score[i] < min )
{
min = score[i];
minindex = i;
}
}
#undef VDROP_FACTOR
#define VDROP_FACTOR minindex * 0.1f
#endif
if ( lowbatt ) hyst = HYST;
else hyst = 0.0f;
vbatt_comp = vbattfilt + (float) VDROP_FACTOR * thrfilt;
if ( vbatt_comp <(float) VBATTLOW + hyst ) lowbatt = 1;
else lowbatt = 0;
// led flash logic
if (rxmode != RX_MODE_BIND)
{
// non bind
if (failsafe)
{
if (lowbatt)
ledflash(500000, 8);
else
ledflash(500000, 15);
}
else
{
if (lowbatt)
ledflash(500000, 8);
else
{
if (ledcommand)
{
if (!ledcommandtime)
ledcommandtime = gettime();
if (gettime() - ledcommandtime > 500000)
{
ledcommand = 0;
ledcommandtime = 0;
}
ledflash(100000, 8);
}
else
{
if ( aux[LEDS_ON] )
ledon( 255);
else
ledoff( 255);
}
}
}
}
else
{ // bind mode
ledflash(100000 + 500000 * (lowbatt), 12);
}
// rgb strip logic
#if (RGB_LED_NUMBER > 0)
extern void rgb_led_lvc( void);
rgb_led_lvc( );
#endif
#ifdef BUZZER_ENABLE
buzzer();
#endif
#ifdef FPV_ON
static int fpv_init = 0;
if ( rxmode == RX_MODE_NORMAL && ! fpv_init ) {
fpv_init = gpio_init_fpv();
}
if ( fpv_init ) {
if ( failsafe ) {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, Bit_RESET );
} else {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, aux[ FPV_ON ] ? Bit_SET : Bit_RESET );
}
}
#endif
checkrx();
// loop time 1ms
while ((gettime() - maintime) < (1000 - 22) )
delay(10);
} // end loop
}
