void check_radio()
{
int temp = xn_readreg( 0x0f); // rx address pipe 5
// should be 0xc6
extern void failloop( int);
if ( temp != 0xc6) failloop(3);
}
void time_init()
{
if (SysTick_Config2(48000000 / 8))
{ // not able to set divider
failloop(5);
while (1) ;
}
// NVIC_SetPriority(SysTick_IRQn, 0x00);
}
void time_init()
{
// 72000000 / (8 (internal div) * 9 )
if (SysTick_Config(SystemCoreClock / (9) )) //1sec interrupts
{// not able to set divider
failloop(5);
while (1);
}
NVIC_SetPriority(SysTick_IRQn, 0x00);
}
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
}
void UsageFault_Handler(void)
{
failloop(5);
}
void BusFault_Handler(void)
{
failloop(5);
}
void MemManage_Handler(void)
{
failloop(5);
}
void HardFault_Handler(void)
{
failloop(5);
}
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();
#ifdef SERIAL
printf("\n clock source:");
#endif
if (RCC_GetCK_SYSSource() == 8)
{
#ifdef SERIAL
printf(" PLL \n");
#endif
}
else
{
#ifdef SERIAL
if (RCC_GetCK_SYSSource() == 0)
printf(" HSI \n");
else
printf(" OTHER \n");
#endif
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;
float vbattfilt = 0.0;
while (count < 64)
{
vbattfilt += adc_read(1);
count++;
}
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();
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 battadc = adc_read(1);
// 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.9968); // 0.5 sec at 1.6ms loop time
lpf(&vbattfilt, battadc, 0.9968);
if (vbattfilt + VDROP_FACTOR * thrfilt < VBATTLOW)
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
ledon(255);
}
}
}
else
{ // bind mode
ledflash(100000 + 500000 * (lowbatt), 12);
}
checkrx();
#ifdef DEBUG
elapsedtime = gettime() - maintime;
#endif
// loop time 1ms
while ((gettime() - maintime) < 1000)
delay(10);
} // end loop
}
void rx_init()
{
// always on (CH_ON) channel set 1
aux[AUXNUMBER - 2] = 1;
// always off (CH_OFF) channel set 0
aux[AUXNUMBER - 1] = 0;
#ifdef AUX1_START_ON
aux[CH_AUX1] = 1;
#endif
#ifdef AUX4_START_ON
aux[CH_AUX4] = 1;
#endif
#ifdef RADIO_XN297L
#define XN_TO_RX B10001111
#define XN_TO_TX B10000010
#define XN_POWER B00111111
#endif
#ifdef RADIO_XN297
static uint8_t bbcal[6] = { 0x3f , 0x4c , 0x84 , 0x6F , 0x9c , 0x20 };
writeregs( bbcal , sizeof(bbcal) );
// new values
static uint8_t rfcal[8] = { 0x3e , 0xc9 , 0x9a , 0xA0 , 0x61 , 0xbb , 0xab , 0x9c };
writeregs( rfcal , sizeof(rfcal) );
static uint8_t demodcal[6] = { 0x39 , 0x0b , 0xdf , 0xc4 , 0xa7 , 0x03};
//static uint8_t demodcal[6] = { 0x39 , 0x0b , 0xdf , 0xc4 , B00100111 , B00000000};
writeregs( demodcal , sizeof(demodcal) );
#define XN_TO_RX B00001111
#define XN_TO_TX B00000010
//#define XN_POWER B00000111 // disabled by silverAG for SilverVISE - value is added from config.h
// SilverVISE - start:
#ifdef TX_POWER_GENERAL
// use value from config.h
#define XN_POWER TX_POWER_GENERAL
#else
#define XN_POWER B00000111
#endif
// SilverVISE - end
#endif
bleinit();
delay(100);
int rxaddress[5] = { 0 , 0 , 0 , 0 , 0 };
xn_writerxaddress( rxaddress);
xn_writereg( EN_AA , 0 ); // aa disabled
xn_writereg( EN_RXADDR , 1 ); // pipe 0 only
xn_writereg( RF_SETUP , XN_POWER); // lna high current on ( better performance )
xn_writereg( RX_PW_P0 , 15 ); // payload size
xn_writereg( SETUP_RETR , 0 ); // no retransmissions ( redundant?)
xn_writereg( SETUP_AW , 3 ); // address size (5 bits)
xn_command( FLUSH_RX);
xn_writereg( RF_CH , 0 ); // bind on channel 0
#ifdef RADIO_XN297L
xn_writereg( 0x1d, B00111000 ); // 64 bit payload , software ce
spi_cson();
spi_sendbyte( 0xFD); // internal CE high command
spi_sendbyte( 0); // required for above
spi_csoff();
#endif
#ifdef RADIO_XN297
xn_writereg( 0x1d, B00011000 ); // 64 bit payload , software ce
#endif
xn_writereg( 0 , XN_TO_RX ); // power up, crc enabled, rx mode
#ifdef RADIO_CHECK
int rxcheck = xn_readreg( 0x0f); // rx address pipe 5
// should be 0xc6
extern void failloop( int);
if ( rxcheck != 0xc6) failloop(3);
#endif
}
int main(void)
{
clk_init();
delay(1000);
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();
#ifdef SERIAL
printf( "\n clock source:" );
#endif
if ( RCC_GetCK_SYSSource() == 8)
{
#ifdef SERIAL
printf( " PLL \n" );
#endif
}
else
{
#ifdef SERIAL
if ( RCC_GetCK_SYSSource() == 0) printf( " HSI \n" );
else printf( " OTHER \n" );
#endif
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;
while ( count < 64 )
{
vbattfilt += adc_read(1);
count++;
}
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 < (float) STOP_LOWBATTERY_TRESH) failloop(2);
#endif
gyro_cal();
extern unsigned int liberror;
if ( liberror )
{
#ifdef SERIAL
printf( "ERROR: I2C \n" );
#endif
failloop(7);
}
static unsigned lastlooptime;
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
#ifdef DEBUG
static float timefilt;
#endif
while(1)
{
// gettime() needs to be called at least once per second
unsigned long time = gettime();
looptime = ((uint32_t)( time - lastlooptime));
if ( looptime <= 0 ) looptime = 1;
looptime = looptime * 1e-6f;
if ( looptime > 0.02f ) // max loop 20ms
{
failloop( 3);
//endless loop
}
#ifdef DEBUG
totaltime += looptime;
lpf ( &timefilt , looptime, 0.998 );
#endif
lastlooptime = time;
if ( liberror > 20)
{
failloop(8);
// endless loop
}
checkrx();
gyro_read();
control();
// battery low logic
static int lowbatt = 0;
float hyst;
float battadc = adc_read(1);
// 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);
if ( lowbatt ) hyst = HYST;
else hyst = 0.0f;
if ( vbattfilt + (float) VDROP_FACTOR * thrfilt <(float) VBATTLOW + hyst ) lowbatt = 1;
else lowbatt = 0;
// led flash logic
if ( rxmode == 0)
{// bind mode
ledflash ( 100000+ 500000*(lowbatt) , 12);
}else
{// non bind
if ( failsafe)
{
if ( lowbatt )
ledflash ( 500000 , 8);
else
ledflash ( 500000, 15);
}
else
{
if ( lowbatt)
ledflash ( 500000 , 8);
else ledon( 255);
}
}
// the delay is required or it becomes endless loop ( truncation in time routine)
while ( (gettime() - time) < 1000 ) delay(10);
}// end loop
}
