void leds_blink_cycles(unsigned char state, unsigned int on_time, unsigned int off_time, unsigned char cycles)
{
for(uint8_t i=0;i<cycles;i++) {
leds_set(state);
lib_timers_delaymilliseconds(on_time);
leds_set(~state);
lib_timers_delaymilliseconds(off_time);
}
}
void init_gyro() {
lib_timers_delaymilliseconds(100);
lib_i2c_writereg(ITG3200_ADDRESS, 0x3E, 0x80); //register: Power Management -- value: reset device
lib_timers_delaymilliseconds(5);
lib_i2c_writereg(ITG3200_ADDRESS, 0x16, 0x18 + ITG3200_DLPF_CFG); //register: DLPF_CFG - low pass filter configuration
lib_timers_delaymilliseconds(5);
lib_i2c_writereg(ITG3200_ADDRESS, 0x3E, 0x03); //register: Power Management -- value: PLL with Z Gyro reference
lib_timers_delaymilliseconds(100);
}
void initrx(void)
{
NRF24L01_Initialize();
reset_beken();
// 2-bytes CRC, radio off
uint8_t config = BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PRIM_RX);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, config);
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
// NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08); // Channel 8 - bind
//NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
NRF24L01_SetBitrate(NRF24L01_BR_250K); //250k for longer range.
NRF24L01_SetPower(TXPOWER_100mW);
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, V2X2_PAYLOAD_SIZE); // bytes of data payload for pipe 0
NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
uint8_t rx_tx_addr[] = {0x66, 0x88, 0x68, 0x68, 0x68};
// uint8_t rx_p1_addr[] = {0x88, 0x66, 0x86, 0x86, 0x86};
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
// NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, rx_p1_addr, 5);
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
initialize_beken();
lib_timers_delaymilliseconds(50);
NRF24L01_FlushTx();
NRF24L01_FlushRx();
rf_ch_num = 0;
// Turn radio power on
config |= BV(NRF24L01_00_PWR_UP);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, config);
// delayMicroseconds(150);
lib_timers_delaymilliseconds(1); // 6 times more than needed
valid_packets = missed_packets = bad_packets = 0;
if (usersettings.boundprotocol == PROTO_NONE) {
bind_phase = PHASE_NOT_BOUND;
prepare_to_bind();
} else {
// Prepare to listen to bound protocol, if fails
// try to bind
bind_phase = PHASE_JUST_BOUND;
set_bound();
}
switch_channel();
}
void init_gyro() {
lib_i2c_writereg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
lib_timers_delaymilliseconds(5);
lib_i2c_writereg(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
lib_i2c_writereg(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
lib_i2c_writereg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
}
void initgyro(void)
{
lib_i2c_writereg(MPU3050_ADDRESS, 0x3E, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
lib_timers_delaymilliseconds(5);
lib_i2c_writereg(MPU3050_ADDRESS, 0x3E, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
lib_i2c_writereg(MPU3050_ADDRESS, 0x16, MPU3050_DLPF_CFG + 0x18); // Gyro CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => GYRO bandwidth = 256Hz); -- FS_SEL = 3: Full scale set to 2000 deg/sec
lib_i2c_writereg(MPU3050_ADDRESS, 0x15, SAMPLE_RATE_DIVISOR);
}
void initacc()
{
lib_timers_delaymilliseconds(10);
//default range 2G: 1G = 4096 unit.
lib_i2c_writereg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0 -- value: set bit ee_w to 1 to enable writing
lib_timers_delaymilliseconds(5);
unsigned char control = lib_i2c_readreg(BMA180_ADDRESS, 0x20);
control = control & 0x0F; // save tcs register
control = control | (0x01 << 4); // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 20Hz
lib_i2c_writereg(BMA180_ADDRESS, 0x20, control);
lib_timers_delaymilliseconds(5);
control = lib_i2c_readreg(BMA180_ADDRESS, 0x30);
control = control & 0xFC; // save tco_z register
control = control | 0x00; // set mode_config to 0
lib_i2c_writereg(BMA180_ADDRESS, 0x30, control);
lib_timers_delaymilliseconds(5);
control = lib_i2c_readreg(BMA180_ADDRESS, 0x35);
control = control & 0xF1; // save offset_x and smp_skip register
control = control | (0x05 << 1); // set range to 8G
lib_i2c_writereg(BMA180_ADDRESS, 0x35, control);
lib_timers_delaymilliseconds(5);
}
void initacc(void)
{
lib_timers_delaymilliseconds(10);
// Mode register: standby mode
lib_i2c_writereg( MC3210_ADDRESS, 0x07, 0x03);
// INTEN register: disable interrupts
lib_i2c_writereg( MC3210_ADDRESS, 0x06, 0x00);
// OUTCFG register:
// Select +/- 8g range, 14-bit resolution
// Low-pass filter set to 64 Hz bandwidth
// (GINT interrupt updates at LPF bandwidth setting)
lib_i2c_writereg( MC3210_ADDRESS, 0x20, 0x8F + ACC_LOW_PASS_FILTER);
// Mode register: wake mode
lib_i2c_writereg( MC3210_ADDRESS, 0x07, 0x01);
}
void init_a7105(void) {
uint8_t i;
uint8_t if_calibration1;
uint8_t vco_calibration0;
uint8_t vco_calibration1;
lib_timers_delaymilliseconds(10); //wait 10ms for A7105 wakeup
A7105_Reset(); //reset A7105
A7105_WriteID(0x5475c52A); //A7105 id
A7105_ReadID((uint8_t*)&aid);
// Serial.print(aid[0], HEX);
// Serial.print(aid[1], HEX);
// Serial.print(aid[2], HEX);
// Serial.print(aid[3], HEX);
// Serial.println("fin id");
for (i = 0; i < 0x33; i++) {
if (A7105_regs[i] != 0xff)
A7105_WriteRegister(i, A7105_regs[i]);
}
A7105_Strobe(A7105_STANDBY); //stand-by
A7105_WriteRegister(0x02, 0x01);
while (A7105_ReadRegister(0x02)) {
if_calibration1 = A7105_ReadRegister(0x22);
if (if_calibration1 & 0x10) { //do nothing
}
}
A7105_WriteRegister(0x24, 0x13);
A7105_WriteRegister(0x26, 0x3b);
A7105_WriteRegister(0x0F, 0x00); //channel 0
A7105_WriteRegister(0x02, 0x02);
while (A7105_ReadRegister(0x02)) {
vco_calibration0 = A7105_ReadRegister(0x25);
if (vco_calibration0 & 0x08) { //do nothing
}
}
A7105_WriteRegister(0x0F, 0xA0);
A7105_WriteRegister(0x02, 0x02);
while (A7105_ReadRegister(0x02)) {
vco_calibration1 = A7105_ReadRegister(0x25);
if (vco_calibration1 & 0x08) { //do nothing
}
}
A7105_WriteRegister(0x25, 0x08);
A7105_Strobe(A7105_STANDBY); //stand-by
}
// It all starts here:
int main(void) {
// start with default user settings in case there's nothing in eeprom
default_user_settings();
// try to load settings from eeprom
read_user_settings_from_eeprom();
// set our LED as a digital output
lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT);
//initialize the libraries that require initialization
lib_timers_init();
lib_i2c_init();
// pause a moment before initializing everything. To make sure everything is powered up
lib_timers_delaymilliseconds(100);
// initialize all other modules
init_rx();
init_outputs();
serial_init();
init_gyro();
init_acc();
init_baro();
init_compass();
init_gps();
init_imu();
// set the default i2c speed to 400 KHz. If a device needs to slow it down, it can, but it should set it back.
lib_i2c_setclockspeed(I2C_400_KHZ);
// initialize state
global.state.armed=0;
global.state.calibratingCompass=0;
global.state.calibratingAccAndGyro=0;
global.state.navigationMode=NAVIGATION_MODE_OFF;
global.failsafeTimer=lib_timers_starttimer();
// run loop
for(;;) {
// check to see what switches are activated
check_checkbox_items();
// check for config program activity
serial_check_for_action();
calculate_timesliver();
// run the imu to estimate the current attitude of the aircraft
imu_calculate_estimated_attitude();
// arm and disarm via rx aux switches
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes
if (!global.state.armed) {
if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) {
global.state.armed=1;
#if (GPS_TYPE!=NO_GPS)
navigation_set_home_to_current_location();
#endif
global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX];
global.home.location.altitude=global.baroRawAltitude;
}
} else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0;
}
#if (GPS_TYPE!=NO_GPS)
// turn on or off navigation when appropriate
if (global.state.navigationMode==NAVIGATION_MODE_OFF) {
if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on
navigation_set_destination(global.home.location.latitude,global.home.location.longitude);
global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME;
} else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on
navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude);
global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD;
}
} else { // we are currently navigating
// turn off navigation if desired
if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) {
global.state.navigationMode=NAVIGATION_MODE_OFF;
// we will be turning control back over to the pilot.
reset_pilot_control();
}
}
#endif
// read the receiver
read_rx();
// turn on the LED when we are stable and the gps has 5 satelites or more
#if (GPS_TYPE==NO_GPS)
lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON);
#else
lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON);
#endif
// get the angle error. Angle error is the difference between our current attitude and our desired attitude.
// It can be set by navigation, or by the pilot, etc.
fixedpointnum angleError[3];
// let the pilot control the aircraft.
get_angle_error_from_pilot_input(angleError);
#if (GPS_TYPE!=NO_GPS)
// read the gps
unsigned char gotNewGpsReading=read_gps();
// if we are navigating, use navigation to determine our desired attitude (tilt angles)
if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating
navigation_set_angle_error(gotNewGpsReading,angleError);
}
#endif
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) {
// We are probably on the ground. Don't accumnulate error when we can't correct it
reset_pilot_control();
// bleed off integrated error by averaging in a value of zero
lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
}
#ifndef NO_AUTOTUNE
// let autotune adjust the angle error if the pilot has autotune turned on
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) {
autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning
} else {
autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning
}
#endif
// This gets reset every loop cycle
// keep a flag to indicate whether we shoud apply altitude hold. The pilot can turn it on or
// uncrashability/autopilot mode can turn it on.
global.state.altitudeHold=0;
if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) {
global.state.altitudeHold=1;
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) {
// we just turned on alt hold. Remember our current alt. as our target
global.altitudeHoldDesiredAltitude=global.altitude;
global.integratedAltitudeError=0;
}
}
fixedpointnum throttleOutput;
#ifndef NO_AUTOPILOT
// autopilot is available
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) {
// let autopilot know to transition to the starting state
autopilot(AUTOPILOT_STARTING);
} else {
// autopilot normal run state
autopilot(AUTOPILOT_RUNNING);
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
// tell autopilot that we just stopped autotuning
autopilot(AUTOPILOT_STOPPING);
} else {
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
}
#else
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
#endif
#ifndef NO_UNCRASHABLE
uncrashable(gotNewGpsReading,angleError,&throttleOutput);
#endif
#if (BAROMETER_TYPE!=NO_BAROMETER)
// check for altitude hold and adjust the throttle output accordingly
if (global.state.altitudeHold) {
global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver);
lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high
// do pid for the altitude hold and add it to the throttle output
throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]);
}
#endif
#ifndef NO_AUTOTHROTTLE
if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) {
if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) {
// Divide the throttle by the throttleOutput by the z component of the down vector
// This is probaly the slow way, but it's a way to do fixed point division
fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]);
recriprocal=lib_fp_multiply(recriprocal,recriprocal);
throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA;
}
}
#endif
#ifndef NO_FAILSAFE
// if we don't hear from the receiver for over a second, try to land safely
if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) {
throttleOutput=FPFAILSAFEMOTOROUTPUT;
// make sure we are level!
angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX];
angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX];
}
#endif
// calculate output values. Output values will range from 0 to 1.0
// calculate pid outputs based on our angleErrors as inputs
fixedpointnum pidOutput[3];
// Gain Scheduling essentialy modifies the gains depending on
// throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
// 1.0 when at mid throttle, and .5 when at zero throttle. This helps
// eliminate the wobbles when decending at low throttle.
fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
for (int x=0;x<3;++x) {
global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver);
// don't let the integrated error get too high (windup)
lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT);
// do the attitude pid
pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4);
// add gain scheduling.
pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]);
}
lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1
compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes
#if (NUM_SERVOS>0)
// do not update servos during unarmed calibration of sensors which are sensitive to vibration
if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs();
#endif
write_motor_outputs();
}