void spi_sendIntPacket_delay(uint8_t t, int16_t v) {
spi_sendIntPacket(t,v);
bcm2835_delay(5); //should be bigger than the AVR controller loop
}
void sendConfig() {
if (verbose) printf("AVRSPI: Sending config\n");
spi_sendIntPacket(3,0); //initial mode
spi_sendIntPacket(2 ,log_mode); //log mode
int gyro_orientation = inv_orientation_matrix_to_scalar(config.gyro_orient);
spi_sendIntPacket(4,gyro_orientation);
spi_sendIntPacket(9,config.mpu_addr);
spi_sendIntPacket(17,config.throttle_min);
spi_sendIntPacket(18,config.throttle_inflight);
spi_sendIntPacket(19,config.throttle_midflight);
uint8_t motor_order =
(config.motor_pin[0]) |
(config.motor_pin[1] << 2) |
(config.motor_pin[2] << 4) |
(config.motor_pin[3] << 6);
spi_sendIntPacket(5,motor_order);
spi_sendIntPacket(69,config.baro_f);
//PIDS
for (int i=0;i<3;i++)
for (int j=0;j<5;j++) {
spi_sendIntPacket(100+i*10+j,config.r_pid[i][j]);
spi_sendIntPacket(200+i*10+j,config.s_pid[i][j]);
}
for (int i=0;i<5;i++) {
spi_sendIntPacket(70+i,config.accel_pid[i]);
spi_sendIntPacket(80+i,config.alt_pid[i]);
spi_sendIntPacket(90+i,config.vz_pid[i]);
}
spi_sendIntPacket(130,config.a_pid[0]);
spi_sendIntPacket(255,2);
if (verbose) printf("AVRSPI: Config sent.\n");
}
void spi_sendMsg(struct avr_msg *m) {
spi_sendIntPacket(m->t,m->v);
bcm2835_delay(2);
}
