void sonarInit(const sonarHardware_t *sonarHardware)
{
sonarRange_t sonarRange;
hcsr04_init(sonarHardware, &sonarRange);
sensorsSet(SENSOR_SONAR);
sonarMaxRangeCm = sonarRange.maxRangeCm;
sonarCfAltCm = sonarMaxRangeCm / 2;
sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2;
sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD);
sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos;
calculatedAltitude = SONAR_OUT_OF_RANGE;
}
void system_board_init(void)
{
/* This function is meant to contain board-specific initialization code
* for, e.g., the I/O pins. The initialization can rely on application-
* specific board configuration, found in conf_board.h.
*/
io_init();
timers_init();
servo_init();
bt_init();
// adc_init();
hcsr04_init();
}
int main(void)
{
HCSR04LP sensor;
sensor.trigger_pin_number = TRIGGER_PIN;
sensor.echo_pin_number = ECHO_PIN;
sensor.power_pin_number = POWER_PIN;
hcsr04_init(&sensor);
turn_on(&sensor);
nrf_delay_us(1000000);
volatile double result = 0.0;
result = get_dist(&sensor);
turn_off(&sensor);
return (int)result;
}
void Sonar_init(void)
{
hcsr04_init(sonar_rc78);
sensorsSet(SENSOR_SONAR);
sonarAlt = 0;
}
void mwii_init(void){
//soc_init();
time_init();
uart_init(0, 38400, uart_buffer[0], 64, uart_buffer[1], 64);
// setup printf stuff (specific to avr-libc)
fdev_set_udata(&uart_fd, uart_get_serial_interface(0));
stdout = &uart_fd;
stderr = &uart_fd;
gpio_init();
//spi_init();
avr_i2c_init(0);
pwm_init();
//adc0_init_default();
sei();
/*
// setup stdout and stderr (avr-libc specific)
fdev_setup_stream(stdout, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW);
fdev_set_udata(stdout, uart_get_serial_interface(0));
fdev_setup_stream(stderr, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW);
fdev_set_udata(stderr, uart_get_serial_interface(0));
*/
// first thing must enable interrupts
//kprintf("BOOT\n");
gpio_configure(GPIO_MWII_LED, GP_OUTPUT);
//gpio_set(GPIO_MWII_LED);
gpio_configure(GPIO_MWII_MOTOR0, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR1, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR2, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR3, GP_OUTPUT);
gpio_configure(GPIO_MWII_RX0, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX1, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX2, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX3, GP_INPUT | GP_PULLUP | GP_PCINT);
// calibrate escs
//mwii_calibrate_escs();
// set initial motor speeds
mwii_write_motors(MINCOMMAND, MINCOMMAND, MINCOMMAND, MINCOMMAND);
brd->gpio0 = gpio_get_parallel_interface();
brd->twi0 = avr_i2c_get_interface(0);
/*
// I2C scanner
for(int c = 0; c < 255; c++){
uint8_t buf[2];
i2c_start_read(brd->twi0, c, buf, 1);
if(i2c_stop(brd->twi0) == 1 && c & 1){
kprintf("Device %x@i2c\n", c >> 1);
}
delay_us(10000);
}
*/
gpio_set(GPIO_MWII_LED);
i2cblk_init(&brd->mpublk, brd->twi0, MPU6050_ADDR, 8, I2CBLK_IADDR8);
mpu6050_init(&brd->mpu, i2cblk_get_interface(&brd->mpublk));
//kdebug("MPU6050: %s\n", ((mpu6050_probe(&brd->mpu))?"found":"not found!"));
i2cblk_init(&brd->bmpblk, brd->twi0, BMP085_ADDR, 8, I2CBLK_IADDR8);
bmp085_init(&brd->bmp, i2cblk_get_interface(&brd->bmpblk));
//kdebug("BMP085: found\n");
i2cblk_init(&brd->hmcblk, brd->twi0, HMC5883L_ADDR, 8, I2CBLK_IADDR8);
hmc5883l_init(&brd->hmc, i2cblk_get_interface(&brd->hmcblk));
gpio_clear(GPIO_MWII_LED);
hcsr04_init(&brd->hcsr, brd->gpio0, GPIO_MWII_HCSR_TRIGGER, GPIO_MWII_HCSR_ECHO);
ASYNC_PROCESS_INIT(&brd->process, mwii_task);
ASYNC_QUEUE_WORK(&ASYNC_GLOBAL_QUEUE, &brd->process);
//mwii_calibrate_mpu6050();
// let the escs init as well
delay_us(500000L);
}
void sonarInit(const sonarHardware_t *sonarHardware)
{
hcsr04_init(sonarHardware);
sensorsSet(SENSOR_SONAR);
calculatedAltitude = -1;
}
