void platform_clock_init(void)
{
clk_init(msm_clocks_msm8909, ARRAY_SIZE(msm_clocks_msm8909));
}
void platform_clock_init(void)
{
clk_init(msm_clocks_fsm9010, ARRAY_SIZE(msm_clocks_fsm9010));
}
int main(void)
{
unsigned int XXL,YXL,ZXL,XGL,YGL,ZGL;
unsigned char XYZ_buffer[100];
clk_init();
port_init();
USART_INIT();
// spi_init();
/* Init SS pin as output with wired AND and pull-up. */
PORTC.DIRSET = PIN0_bm;
PORTC.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc;
PORTC.DIRSET = PIN1_bm;
PORTC.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTC.OUTSET = PIN0_bm;
PORTC.OUTSET = PIN1_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTC;
/* Initialize SPI master on port C. */
SPI_MasterInit(&spiMasterC,
&SPIC,
&PORTC,
false,
SPI_MODE_3_gc,
SPI_INTLVL_OFF_gc,
false,
SPI_PRESCALER_DIV16_gc);
/* Initialize ACC & Gyro */
Init_L3G4200DH();
Init_LIS331DLH();
/* Read Sensor data */
while(true) {
/* Create data packet (SS to slave by PC0). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
ACC_DATA,
NUM_BYTES,
&PORTC,
PIN0_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN0_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN0_bm);
/* Create data packet (SS to slave by PC1). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
GYRO_DATA,
NUM_BYTES,
&PORTC,
PIN1_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN1_bm);
ACC_DATA[2]=ACC_DATA[2]+0x80;
ACC_DATA[4]=ACC_DATA[4]+0x80;
ACC_DATA[6]=ACC_DATA[6]+0x80;
XXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1];
YXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3];
ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5];
GYRO_DATA[2]=GYRO_DATA[2]+0x80;
GYRO_DATA[4]=GYRO_DATA[4]+0x80;
GYRO_DATA[6]=GYRO_DATA[6]+0x80;
XGL= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1];
YGL= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3];
ZGL= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5];
sprintf((char*)XYZ_buffer,"[XXL%6u][YXL%6u][ZXL%6u][XGY%6u][YGY%6u][ZGY%6u]\r\n",XXL,YXL,ZXL,XGL,YGL,ZGL);
//uartSendTXbit('3');
//sprintf((char*)XYZ_buffer,"abcdefgefgh12341\n\r");
uartSendTX(XYZ_buffer);
}
while(true) {
nop();
}
}
void platform_clock_init(void)
{
if (platform_is_8974ac())
msm8974_ac_clock_override();
clk_init(msm_clocks_8974, ARRAY_SIZE(msm_clocks_8974));
}
void platform_clock_init(void)
{
if (platform_is_msm8956())
msm8956_clock_override();
clk_init(msm_clocks_8952, ARRAY_SIZE(msm_clocks_8952));
}
void platform_clock_init(void)
{
clk_init(msm_clocks_8226, ARRAY_SIZE(msm_clocks_8226));
}
int __init omap1_clk_init(void)
{
struct omap_clk *c;
const struct omap_clock_config *info;
int crystal_type = 0; /* Default 12 MHz */
u32 reg, cpu_mask;
#ifdef CONFIG_DEBUG_LL
/*
* Resets some clocks that may be left on from bootloader,
* but leaves serial clocks on.
*/
omap_writel(0x3 << 29, MOD_CONF_CTRL_0);
#endif
/* USB_REQ_EN will be disabled later if necessary (usb_dc_ck) */
reg = omap_readw(SOFT_REQ_REG) & (1 << 4);
omap_writew(reg, SOFT_REQ_REG);
if (!cpu_is_omap15xx())
omap_writew(0, SOFT_REQ_REG2);
clk_init(&omap1_clk_functions);
/* By default all idlect1 clocks are allowed to idle */
arm_idlect1_mask = ~0;
for (c = omap_clks; c < omap_clks + ARRAY_SIZE(omap_clks); c++)
clk_preinit(c->lk.clk);
cpu_mask = 0;
if (cpu_is_omap16xx())
cpu_mask |= CK_16XX;
if (cpu_is_omap1510())
cpu_mask |= CK_1510;
if (cpu_is_omap7xx())
cpu_mask |= CK_7XX;
if (cpu_is_omap310())
cpu_mask |= CK_310;
for (c = omap_clks; c < omap_clks + ARRAY_SIZE(omap_clks); c++)
if (c->cpu & cpu_mask) {
clkdev_add(&c->lk);
clk_register(c->lk.clk);
}
/* Pointers to these clocks are needed by code in clock.c */
api_ck_p = clk_get(NULL, "api_ck");
ck_dpll1_p = clk_get(NULL, "ck_dpll1");
ck_ref_p = clk_get(NULL, "ck_ref");
info = omap_get_config(OMAP_TAG_CLOCK, struct omap_clock_config);
if (info != NULL) {
if (!cpu_is_omap15xx())
crystal_type = info->system_clock_type;
}
if (cpu_is_omap7xx())
ck_ref.rate = 13000000;
if (cpu_is_omap16xx() && crystal_type == 2)
ck_ref.rate = 19200000;
pr_info("Clocks: ARM_SYSST: 0x%04x DPLL_CTL: 0x%04x ARM_CKCTL: "
"0x%04x\n", omap_readw(ARM_SYSST), omap_readw(DPLL_CTL),
omap_readw(ARM_CKCTL));
/* We want to be in syncronous scalable mode */
omap_writew(0x1000, ARM_SYSST);
/*
* Initially use the values set by bootloader. Determine PLL rate and
* recalculate dependent clocks as if kernel had changed PLL or
* divisors. See also omap1_clk_late_init() that can reprogram dpll1
* after the SRAM is initialized.
*/
{
unsigned pll_ctl_val = omap_readw(DPLL_CTL);
ck_dpll1.rate = ck_ref.rate; /* Base xtal rate */
if (pll_ctl_val & 0x10) {
/* PLL enabled, apply multiplier and divisor */
if (pll_ctl_val & 0xf80)
ck_dpll1.rate *= (pll_ctl_val & 0xf80) >> 7;
ck_dpll1.rate /= ((pll_ctl_val & 0x60) >> 5) + 1;
} else {
/* PLL disabled, apply bypass divisor */
switch (pll_ctl_val & 0xc) {
case 0:
break;
case 0x4:
ck_dpll1.rate /= 2;
break;
default:
ck_dpll1.rate /= 4;
break;
}
}
}
void platform_clock_init(void)
{
clk_init(msm_clocks_8084, ARRAY_SIZE(msm_clocks_8084));
}
void platform_clock_init(void)
{
clk_init(msm_8994_clocks, ARRAY_SIZE(msm_8994_clocks));
}
void init()
{
clk_init();
uart_init(UART_B9600);
print_init(uart_send, uart_send_byte);
}
void platform_clock_init(void)
{
clk_init(msm_clocks_9640, ARRAY_SIZE(msm_clocks_9640));
}
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
}
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 msm_clocks_init()
{
clk_ops_pll.enable = sr_pll_clk_enable;
clk_init(msm_clocks_8960, msm_num_clocks_8960);
}
int __init omap2_clk_init(void)
{
struct prcm_config *prcm;
struct omap_clk *c;
u32 clkrate;
if (cpu_is_omap242x()) {
prcm_clksrc_ctrl = OMAP2420_PRCM_CLKSRC_CTRL;
cpu_mask = RATE_IN_242X;
} else if (cpu_is_omap2430()) {
prcm_clksrc_ctrl = OMAP2430_PRCM_CLKSRC_CTRL;
cpu_mask = RATE_IN_243X;
}
clk_init(&omap2_clk_functions);
for (c = omap24xx_clks; c < omap24xx_clks + ARRAY_SIZE(omap24xx_clks); c++)
clk_preinit(c->lk.clk);
osc_ck.rate = omap2_osc_clk_recalc(&osc_ck);
propagate_rate(&osc_ck);
sys_ck.rate = omap2_sys_clk_recalc(&sys_ck);
propagate_rate(&sys_ck);
for (c = omap24xx_clks; c < omap24xx_clks + ARRAY_SIZE(omap24xx_clks); c++)
if (c->cpu & cpu_mask) {
clkdev_add(&c->lk);
clk_register(c->lk.clk);
omap2_init_clk_clkdm(c->lk.clk);
}
/* Check the MPU rate set by bootloader */
clkrate = omap2xxx_clk_get_core_rate(&dpll_ck);
for (prcm = rate_table; prcm->mpu_speed; prcm++) {
if (!(prcm->flags & cpu_mask))
continue;
if (prcm->xtal_speed != sys_ck.rate)
continue;
if (prcm->dpll_speed <= clkrate)
break;
}
curr_prcm_set = prcm;
recalculate_root_clocks();
printk(KERN_INFO "Clocking rate (Crystal/DPLL/MPU): "
"%ld.%01ld/%ld/%ld MHz\n",
(sys_ck.rate / 1000000), (sys_ck.rate / 100000) % 10,
(dpll_ck.rate / 1000000), (mpu_ck.rate / 1000000)) ;
/*
* Only enable those clocks we will need, let the drivers
* enable other clocks as necessary
*/
clk_enable_init_clocks();
/* Avoid sleeping sleeping during omap2_clk_prepare_for_reboot() */
vclk = clk_get(NULL, "virt_prcm_set");
sclk = clk_get(NULL, "sys_ck");
return 0;
}
int __init omap2_clk_init(void)
{
struct prcm_config *prcm;
struct clk **clkp;
u32 clkrate;
if (cpu_is_omap242x())
cpu_mask = RATE_IN_242X;
else if (cpu_is_omap2430())
cpu_mask = RATE_IN_243X;
clk_init(&omap2_clk_functions);
omap2_osc_clk_recalc(&osc_ck);
omap2_sys_clk_recalc(&sys_ck);
for (clkp = onchip_24xx_clks;
clkp < onchip_24xx_clks + ARRAY_SIZE(onchip_24xx_clks);
clkp++) {
if ((*clkp)->flags & CLOCK_IN_OMAP242X && cpu_is_omap2420()) {
clk_register(*clkp);
continue;
}
if ((*clkp)->flags & CLOCK_IN_OMAP243X && cpu_is_omap2430()) {
clk_register(*clkp);
continue;
}
}
/* Check the MPU rate set by bootloader */
clkrate = omap2_get_dpll_rate_24xx(&dpll_ck);
for (prcm = rate_table; prcm->mpu_speed; prcm++) {
if (!(prcm->flags & cpu_mask))
continue;
if (prcm->xtal_speed != sys_ck.rate)
continue;
if (prcm->dpll_speed <= clkrate)
break;
}
curr_prcm_set = prcm;
recalculate_root_clocks();
printk(KERN_INFO "Clocking rate (Crystal/DPLL/MPU): "
"%ld.%01ld/%ld/%ld MHz\n",
(sys_ck.rate / 1000000), (sys_ck.rate / 100000) % 10,
(dpll_ck.rate / 1000000), (mpu_ck.rate / 1000000)) ;
/*
* Only enable those clocks we will need, let the drivers
* enable other clocks as necessary
*/
clk_enable_init_clocks();
/* Avoid sleeping sleeping during omap2_clk_prepare_for_reboot() */
vclk = clk_get(NULL, "virt_prcm_set");
sclk = clk_get(NULL, "sys_ck");
return 0;
}
void platform_clock_init(void)
{
if (platform_is_msm8939())
msm8939_clock_override();
clk_init(msm_clocks_8916, ARRAY_SIZE(msm_clocks_8916));
}
int __init omap2_clk_init(void)
{
/* struct prcm_config *prcm; */
struct clk **clkp;
/* u32 clkrate; */
u32 cpu_clkflg;
/* REVISIT: Ultimately this will be used for multiboot */
#if 0
if (cpu_is_omap242x()) {
cpu_mask = RATE_IN_242X;
cpu_clkflg = CLOCK_IN_OMAP242X;
clkp = onchip_24xx_clks;
} else if (cpu_is_omap2430()) {
cpu_mask = RATE_IN_243X;
cpu_clkflg = CLOCK_IN_OMAP243X;
clkp = onchip_24xx_clks;
}
#endif
if (cpu_is_omap34xx()) {
cpu_mask = RATE_IN_343X;
cpu_clkflg = CLOCK_IN_OMAP343X;
clkp = onchip_34xx_clks;
/*
* Update this if there are further clock changes between ES2
* and production parts
*/
if (is_sil_rev_equal_to(OMAP3430_REV_ES1_0)) {
/* No 3430ES1-only rates exist, so no RATE_IN_3430ES1 */
cpu_clkflg |= CLOCK_IN_OMAP3430ES1;
} else {
cpu_mask |= RATE_IN_3430ES2;
cpu_clkflg |= CLOCK_IN_OMAP3430ES2;
}
}
clk_init(&omap2_clk_functions);
for (clkp = onchip_34xx_clks;
clkp < onchip_34xx_clks + ARRAY_SIZE(onchip_34xx_clks);
clkp++) {
if ((*clkp)->flags & cpu_clkflg)
clk_register(*clkp);
}
/* REVISIT: Not yet ready for OMAP3 */
#if 0
/* Check the MPU rate set by bootloader */
clkrate = omap2_get_dpll_rate_24xx(&dpll_ck);
for (prcm = rate_table; prcm->mpu_speed; prcm++) {
if (!(prcm->flags & cpu_mask))
continue;
if (prcm->xtal_speed != sys_ck.rate)
continue;
if (prcm->dpll_speed <= clkrate)
break;
}
curr_prcm_set = prcm;
#endif
recalculate_root_clocks();
printk(KERN_INFO "Clocking rate (Crystal/DPLL/ARM core): "
"%ld.%01ld/%ld/%ld MHz\n",
(osc_sys_ck.rate / 1000000), (osc_sys_ck.rate / 100000) % 10,
(core_ck.rate / 1000000), (arm_fck.rate / 1000000));
/*
* Only enable those clocks we will need, let the drivers
* enable other clocks as necessary
*/
clk_enable_init_clocks();
/* Avoid sleeping during omap2_clk_prepare_for_reboot() */
/* REVISIT: not yet ready for 343x */
#if 0
vclk = clk_get(NULL, "virt_prcm_set");
sclk = clk_get(NULL, "sys_ck");
#endif
return 0;
}
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
}