/**
* Initializing the cyrf chip
*/
void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port,
const uint16_t rst_pin)
{
/* Set spi_peripheral and the status */
cyrf->spi_p = spi_p;
cyrf->status = CYRF6936_UNINIT;
cyrf->has_irq = FALSE;
/* Set the spi transaction */
cyrf->spi_t.cpol = SPICpolIdleLow;
cyrf->spi_t.cpha = SPICphaEdge1;
cyrf->spi_t.dss = SPIDss8bit;
cyrf->spi_t.bitorder = SPIMSBFirst;
cyrf->spi_t.cdiv = SPIDiv64;
cyrf->spi_t.input_length = 0;
cyrf->spi_t.output_length = 0;
cyrf->spi_t.input_buf = cyrf->input_buf;
cyrf->spi_t.output_buf = cyrf->output_buf;
cyrf->spi_t.slave_idx = slave_idx;
cyrf->spi_t.select = SPISelectUnselect;
cyrf->spi_t.status = SPITransDone;
/* Reset the CYRF6936 chip (busy waiting) */
gpio_setup_output(rst_port, rst_pin);
gpio_set(rst_port, rst_pin);
sys_time_usleep(100);
gpio_clear(rst_port, rst_pin);
sys_time_usleep(100);
/* Get the MFG ID */
cyrf->status = CYRF6936_GET_MFG_ID;
cyrf->buffer_idx = 0;
cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF);
}
int main(void)
{
main_init();
#if LIMIT_EVENT_POLLING
/* Limit main loop frequency to 1kHz.
* This is a kludge until we can better leverage threads and have real events.
* Without this limit the event flags will constantly polled as fast as possible,
* resulting on 100% cpu load on boards with an (RT)OS.
* On bare metal boards this is not an issue, as you have nothing else running anyway.
*/
uint32_t t_begin = 0;
uint32_t t_diff = 0;
while (1) {
t_begin = get_sys_time_usec();
handle_periodic_tasks();
main_event();
/* sleep remaining time to limit to 1kHz */
t_diff = get_sys_time_usec() - t_begin;
if (t_diff < 1000) {
sys_time_usleep(1000 - t_diff);
}
}
#else
while (1) {
handle_periodic_tasks();
main_event();
}
#endif
return 0;
}
int main(void)
{
mcu_init();
/*
* Init threads
*/
chThdCreateStatic(wa_thd_main_periodic_05, sizeof(wa_thd_main_periodic_05), NORMALPRIO, thd_main_periodic_05, NULL);
while (1) {
/* sleep for 1s */
sys_time_ssleep(1);
main_periodic_1();
/* sleep for 0.42s */
/*
* sys_time_usleep(uint32_t us)
* Use only for up to 2^32/CH_CFG_ST_FREQUENCY-1 usec
* e.g. if CH_CFG_ST_FREQUENCY=10000 use max for 420000 us
* or 420ms, otherwise overflow happens
*/
sys_time_usleep(420000);
main_periodic_15();
}
return 0;
}
void pcap01_init(void)
{
pcap01_trans.status = I2CTransDone;
pcap01Value.status = PCAP01_IDLE;
#ifdef PCAP01_LOAD_FIRMWARE
writePCAP01_firmware();
#endif
pcap01writeRegister(PCAP01_REG0, PCAP01_REG0_VALUE);
pcap01writeRegister(PCAP01_REG1, PCAP01_REG1_VALUE);
pcap01writeRegister(PCAP01_REG2, PCAP01_REG2_VALUE);
pcap01writeRegister(PCAP01_REG3, PCAP01_REG3_VALUE);
pcap01writeRegister(PCAP01_REG4, PCAP01_REG4_VALUE);
pcap01writeRegister(PCAP01_REG5, PCAP01_REG5_VALUE);
pcap01writeRegister(PCAP01_REG6, PCAP01_REG6_VALUE);
pcap01writeRegister(PCAP01_REG7, PCAP01_REG7_VALUE);
pcap01writeRegister(PCAP01_REG8, PCAP01_REG8_VALUE);
pcap01writeRegister(PCAP01_REG9, PCAP01_REG9_VALUE);
pcap01writeRegister(PCAP01_REG10, PCAP01_REG10_VALUE);
pcap01writeRegister(PCAP01_REG11, PCAP01_REG11_VALUE);
pcap01writeRegister(PCAP01_REG12, PCAP01_REG12_VALUE);
pcap01writeRegister(PCAP01_REG13, PCAP01_REG13_VALUE);
pcap01writeRegister(PCAP01_REG14, PCAP01_REG14_VALUE);
pcap01writeRegister(PCAP01_REG15, PCAP01_REG15_VALUE);
pcap01writeRegister(PCAP01_REG16, PCAP01_REG16_VALUE);
pcap01writeRegister(PCAP01_REG17, PCAP01_REG17_VALUE);
pcap01writeRegister(PCAP01_REG18, PCAP01_REG18_VALUE);
pcap01writeRegister(PCAP01_REG19, PCAP01_REG19_VALUE);
pcap01writeRegister(PCAP01_REG20, PCAP01_REG20_VALUE);
PCAP01_Control(PCAP01_IN_RESET);
sys_time_usleep(500000);
PCAP01_Control(PCAP01_START);
}
int main(void)
{
mcu_init();
sys_time_register_timer(0.5, main_periodic_05);
mcu_int_enable();
while (1) {
/* sleep for 1s */
sys_time_usleep(1000000);
main_periodic_1();
/* sleep for 0.5s */
sys_time_usleep(500000);
main_periodic_15();
}
return 0;
}
void writePCAP01_firmware(void)
{
int i = 0;
char testbyte = 44;
uint16_t testaddress = 71;
// Hard reset
PCAP01_Control(PCAP01_PU_RESET);
sys_time_usleep(50000);
//write testbyte
writePCAP01_SRAM(testbyte, testaddress);
//check testbyte
if (readPCAP01_SRAM(testaddress) != testbyte) {
return;
} else {
LED_ON(3);
//Hard reset
PCAP01_Control(PCAP01_PU_RESET);
//write firmware
for (i = 0; i < sizeof(firmware); i++) {
writePCAP01_SRAM(firmware[i], i);
}
//fill with ffs
for (; i < 4029; i++) {
writePCAP01_SRAM(0xff, i);
}
i++;
#ifdef PCAP01_200HZ
//write end bytes of sram
writePCAP01_SRAM(0x04, i++);
writePCAP01_SRAM(0x01, i++);
writePCAP01_SRAM(0x01, i++);
#endif
#ifdef PCAP01_STANDARD
//write end bytes of sram
writePCAP01_SRAM(0x02, i++);
writePCAP01_SRAM(0x01, i++);
writePCAP01_SRAM(0x03, i++);
#endif
}
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
sys_time_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
infrared_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_IMU
imu_init();
#endif
#ifdef USE_AHRS
ahrs_aligner_init();
ahrs_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
settings_init();
/** - start interrupt task */
mcu_int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
void w5100_init( void ) {
// configure the SPI bus.
w5100_spi.slave_idx = W5100_SLAVE_IDX;
w5100_spi.output_length = 4;
w5100_spi.input_length = 4;
w5100_spi.select = SPISelectUnselect;
w5100_spi.cpol = SPICpolIdleLow;
w5100_spi.cpha = SPICphaEdge1;
w5100_spi.dss = SPIDss8bit;
w5100_spi.bitorder = SPIMSBFirst;
w5100_spi.cdiv = SPIDiv64;
chip0.status = W5100StatusUninit;
chip0.curbuf = 0;
w5100_spi.input_buf = &chip0.work_rx[0];
w5100_spi.output_buf = &chip0.work_tx[0];
// wait one second for proper initialization (chip getting powered up).
sys_time_usleep(1000000);
// set DRDY pin
gpio_setup_output(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN);
gpio_clear(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN);
sys_time_usleep(200);
gpio_set(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN);
// allow some time for the chip to wake up.
sys_time_usleep(20000);
// write reset bit into mode register
w5100_set( REG_MR, 1<<RST );
// allow some time to wake up...
sys_time_usleep(20000);
// receive memory size
w5100_set( REG_RX_MEM, 0x55 );
// transmit memory size
w5100_set( REG_TX_MEM, 0x55 );
// Setting the required socket base addresses for reads and writes to/from sockets
for (int i=0; i<SOCKETS; i++) {
SBASE[i] = TXBUF_BASE + SSIZE * i;
RBASE[i] = RXBUF_BASE + RSIZE * i;
}
uint8_t gateway[4];
gateway[0] = ip[0];
gateway[1] = ip[1];
gateway[2] = ip[2];
gateway[3] = 1;
// configure gateway, subnet, mac and ip on "NIC".
w5100_set_buffer( REG_GAR, gateway, 4 );
w5100_set_buffer( REG_SUBR, subnet, 4 );
w5100_set_buffer( REG_SHAR, mac, 6 );
w5100_set_buffer( REG_SIPR, ip, 4 );
// create a socket to send telemetry through.
configure_socket( TELEM_SOCKET, SNMR_MULTI, 1, dport, dest );
// make dest zero and configure socket to receive data
dest[ 0 ] = 0x00;
configure_socket( CMD_SOCKET, 0, dport, dport, dest );
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_BAROMETER
baro_init();
#endif
ins_init();
stateInit();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1./60, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
#ifdef SET_IMU_ZERO_ON_STARTUP
#ifndef SITL
//wait 10secs for init
sys_time_usleep(10000000);
imu_store_bias();
#endif
#endif
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
hw_init();
sys_time_init();
#ifdef LED
led_init();
#endif
#ifdef ADC
adc_init();
#endif
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
ir_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef USE_UART2
Uart2Init();
#endif
#ifdef USE_UART3
Uart3Init();
#endif
#ifdef USE_USB_SERIAL
VCOM_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_I2C0
i2c0_init();
#endif
#ifdef USE_I2C1
i2c1_init();
#endif
#ifdef USE_I2C2
i2c2_init();
#endif
/************* Links initialization ***************/
#if defined USE_SPI
spi_init();
#endif
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
/** - start interrupt task */
int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
void px4flash_event(void)
{
if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
if (!setToBootloaderMode) {
//ignore anything coming from IO if not in bootloader mode (which should be nothing)
} else {
//relay everything from IO to the laptop
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
FLASH_PORT->put_byte(FLASH_PORT->periph, b);
}
}
}
//TODO: check if bootloader timeout was surpassed
if (FLASH_PORT->char_available(FLASH_PORT->periph) && !setToBootloaderMode) {
// TMP TEST
// while (FLASH_PORT->char_available(FLASH_PORT->periph)) {
// unsigned char bla = FLASH_PORT->get_byte(FLASH_PORT->periph);
// FLASH_PORT->put_byte(FLASH_PORT->periph,bla);
// }
// return;
//check whether this is flash related communication, and for who (ap/fbw)
int state = 0;
while (state < 4 && FLASH_PORT->char_available(FLASH_PORT->periph)) {
unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
switch (state) {
case (0) :
if (b == 'p') { state++; } else { return; }
break;
case (1) :
if (b == 'p') { state++; } else { return; }
break;
case (2) :
if (b == 'r') { state++; } else { return; }
break;
case (3) :
if (b == 'z') { state++; } else { return; }
break;
default :
break;
}
}
if (state != 4) {return;}
//TODO: check if/how this interferes with flashing original PX4 firmware
unsigned char target = FLASH_PORT->get_byte(FLASH_PORT->periph);
if (target == '1') { //target ap
//the target is the ap, so reboot to PX4 bootloader
scb_reset_system();
} else { // target fbw
//the target is the fbw, so reboot the fbw and switch to relay mode
//first check if the bootloader has not timeout:
if (sys_time_check_and_ack_timer(px4iobl_tid) || px4ioRebootTimeout) {
px4ioRebootTimeout = TRUE;
sys_time_cancel_timer(px4iobl_tid);
FLASH_PORT->put_byte(FLASH_PORT->periph, 'T');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'I');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'M');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'E');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'U');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'T'); // use 7 chars as answer
return;
} { // FBW OK OK hollay hollay :)
FLASH_PORT->put_byte(FLASH_PORT->periph, 'F');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'B');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'W');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'K');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'K'); // use 7 chars as answer
}
//stop all intermcu communication:
disable_inter_comm(true);
/*
* The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
* In that case:
* 1. enable this define
* 2. build and upload the fmu f4 chip (ap target in pprz center)
* 3. build the io code, and convert the firmware using the following command:
* /home/houjebek/paparazzi/sw/tools/px4/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
* 4. Start the following command:
* /home/houjebek/paparazzi/sw/tools/px4/px_uploader.py --port "/dev/ttyACM0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
* 5a. Either, boot the Pixhawk (reconnect usb) holding the IO reset button until the FMU led stops blinking fast (i.e. exits its own bootloader)
* 5b Or, press the IO reset button on the pixhawk
* 6. Watch the output of the command of step 4, it should recognize the IO bootloader and start flashing. If not try repeating step 5a.
* 7. Don forget to disable the define and upload the ap again :)
*/
// #define progdieshit
#ifndef progdieshit
//send the reboot to bootloader command:
static struct IOPacket dma_packet;
dma_packet.count_code = 0x40 + 0x01;
dma_packet.crc = 0;
dma_packet.page = PX4IO_PAGE_SETUP;
dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL;
dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC;
dma_packet.crc = crc_packet(&dma_packet);
struct IOPacket *pkt = &dma_packet;
uint8_t *p = (uint8_t *)pkt;
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]);
sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side
//the pixhawk IO chip should respond with:
// 0x00 ( PKT_CODE_SUCCESS )
// 0xe5
// 0x32
// 0x0a
//After that, the IO chips reboots into bootloader mode, in which it will stay for a short period
//The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware)
//state machine
state = 0;
while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
switch (state) {
case (0) :
if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; }
break;
case (1) :
if (b == 0xe5) { state++; } else { state = 0; }
break;
case (2) :
if (b == 0x32) { state++; } else { state = 0; }
break;
case (3) :
if (b == 0x0a) { state++; } else { state = 0; }
break;
default :
break;
}
}
#else
int state = 4;
#endif
if (state == 4) {
uart_periph_set_baudrate(PX4IO_PORT->periph, B115200);
/* look for the bootloader for 150 ms */
int ret = 0;
for (int i = 0; i < 15 && !ret ; i++) {
sys_time_usleep(10000);
//send a get_sync command in order to keep the io in bootloader mode
PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC);
//get_sync should be replied with, so check if that happens and
//all other bytes are discarded, hopefully those were not important
//(they may be caused by sending multiple syncs)
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
if (b == PROTO_INSYNC) {
setToBootloaderMode = true;
ret = 1;
break;
}
}
}
if (setToBootloaderMode) {
//if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check)
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);}
}
} else {
FLASH_PORT->put_byte(FLASH_PORT->periph, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, '!');
FLASH_PORT->put_byte(FLASH_PORT->periph, ' '); // use 7 chars as answer
}
}
} else if (FLASH_PORT->char_available(FLASH_PORT->periph)) {
//already in bootloader mode, just directly relay data
while (FLASH_PORT->char_available(FLASH_PORT->periph)) {
unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, b);
}
}
}
