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
}
STATIC_INLINE void main_init(void)
{
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
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 */
mcu_init();
#endif /* SINGLE_MCU */
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#if USE_IMU
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
#endif
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
ins_init();
/************* 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./TELEMETRY_FREQUENCY, 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
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
int main()
{
//uint32_t count;
MSS_GPIO_init();
MSS_GPIO_config( MSS_GPIO_0, MSS_GPIO_OUTPUT_MODE);
MSS_GPIO_config( MSS_GPIO_1, MSS_GPIO_OUTPUT_MODE);
MSS_I2C_init(&g_mss_i2c1 , IMU_ADDRESS_WRITE, MSS_I2C_PCLK_DIV_256 );
imu_init();
//one timer tick is 10ns at 100Mhz
MSS_TIM1_init( MSS_TIMER_PERIODIC_MODE);
MSS_TIM1_enable_irq();
MSS_TIM1_load_background(100000);
MSS_TIM1_start();
MSS_TIM2_init( MSS_TIMER_ONE_SHOT_MODE);
while( 1 ) {
//int16_t x_accl = imu_accl_x();
//int16_t y_accl = imu_accl_y();
//int16_t z_accl = imu_accl_z();
X_GYRO = imu_gyro_x();
Y_ACCL = imu_accl_y();
//uint16_t y_gyro = imu_gyro_y();
//uint16_t z_gyro = imu_gyro_z();
//printf("Avg X: %f\r\n", avg_x);
//printf("Accel X: %i\n\r", x_accl);
//printf("Accel Y: %u\n\r", y_accl);
//printf("Accel Z: %u\n\r", z_accl);
//printf("Gyro X: %u\r\n", x_gyro);
//printf("Gyro Y: %u\r\n", y_gyro);
//printf("Gyro Z: %u\r\n", z_gyro);
//150000 = 1.5ms pulse
//1.49ms pulse = equilibrium point
uint32_t pulsewidth_right = 150000 + (angle/90.0)*100000;
uint32_t pulsewidth_left = 150000 - (angle/90.0)*100000;
uint32_t period = 2000000;
MSS_TIM2_load_immediate(period);
MSS_TIM2_start();
while (MSS_TIM2_get_current_value() > 0) {
if (MSS_TIM2_get_current_value() > (period - pulsewidth_right))
MSS_GPIO_set_output( MSS_GPIO_0, 1);
else
MSS_GPIO_set_output( MSS_GPIO_0, 0);
if (MSS_TIM2_get_current_value() > (period - pulsewidth_left))
MSS_GPIO_set_output( MSS_GPIO_1, 1);
else
MSS_GPIO_set_output( MSS_GPIO_1, 0);
}
}
return 0;
}
int main (void)
{
unsigned char value;
//Configure all ports as inputs
TRISA = 0xFFFF; TRISB = 0xFFFF;// TRISC = 0xFFFF;
//---------------------------------------------------------------------
// OSCILATOR
//---------------------------------------------------------------------
oscilator_init();
//config_load();
//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
uart_init();
printf("Uart Init Ok\n");
//---------------------------------------------------------------------
// STATUS (LED/SOUND)
//---------------------------------------------------------------------
status_init();
// STATUS(BLINK_SLOW,_ON,_ON);
// __delay_ms(10);
// STATUS_INIT;
//---------------------------------------------------------------------
// ADC
//---------------------------------------------------------------------
// adc_init();
// timer_init_ms();
//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
i2c_init();
printf("i2c Init Ok\n");
ITG3200_begin();
printf("ITG3200 Ok\n");
ADXL345_begin();
printf("ADXL Init Ok\n");
//STATUS(BLINK_FAST,_ON,_ON);
printf("Init Gyro\n");
// STATUS_NORMAL;
//__delay_ms(100);
// Acceleromter calibration
int AcclCalibration;
AcclCalibration =1;
//#define AccCali
#ifdef AccCali
while(AcclCalibration){
int i;
double adjAccl;
for(i=0;i<10;i++){
ADXL345_update();
}
float Acclx=getAcclXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gravity=sqrt(Acclx*Acclx+Accly*Accly+Acclz*Acclz);
adjAccl= 1/Gravity;
ADXL345_Scaler =adjAccl * ADXL345_Scaler;
printf("%1.3f %1.3f %1.3f Gravity %1.9f y %1.9f %1.3f\n",Acclx,Accly,Acclz,Gravity,ADXL345_Scaler,adjAccl);
if (Gravity == 1.0000) AcclCalibration=0;
}
printf("Accelerometer Calibrated \n");
#endif
//#define GyroTest
#ifdef GyroTest
while(1){
ITG3200_update();
float GyroX=getGyroXOutput();
float GyroY=getGyroYOutput();
float GyroZ=getGyroZOutput();
int ID=getGyroIDOutput();
float Temp= getGyroTempOutput();
printf("%1.3f %1.3f %1.3f %1.3f %i \n",GyroX,GyroY,GyroZ,Temp,ID);
}
#endif
//---------------------------------------------------------------------
// MOTOR
//---------------------------------------------------------------------
motor_init();
printf("motor init ok\n");
__delay_ms(100);
//#define motor_debug
#ifdef motor_debug
//while(1){
// _LAT(pinMotor0) = 1;
// _LAT(pinMotor1) = 1;
// _LAT(pinMotor2) = 1;
// _LAT(pinMotor3) = 1;
//
// _TRIS(pinMotor0) = 0;
// _TRIS(pinMotor1) = 0;
// _TRIS(pinMotor2) = 0;
// _TRIS(pinMotor3)= 0;
//
//
//}
int i=0;
while(i<50){
_LAT(pinLed1) =! _LAT(pinLed1);
motor_set_duty(0,i);
motor_set_duty(1,i);
motor_set_duty(2,i);
motor_set_duty(3,i);
motor_apply_duty();
__delay_ms(50);
i = i + 1;
// if(i> 90) i = 0;
printf("Motor %x \n");
}
__delay_ms(250);
i=0;
motor_set_duty(0,i);
motor_apply_duty();
motor_set_duty(1,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(2,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(3,i);
__delay_ms(250);
motor_apply_duty();
__delay_ms(250);
#endif
//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();
//#define imu_debug
#ifdef imu_debug
//IMU debug comment out for production
float interval_ms=0;
while (1){
ITG3200_update();
ADXL345_update();
//interval_ms = timer_dt();
// if(interval_ms > 0 ){
//we have fresh adc samples
// printf("Time %lu ",interval_us);
imu_update(interval_ms);
float* K = dcmEst[2]; //K(body) vector from DCM matrix
float pitch_roll = acos(K[2]); //total pitch and roll, angle necessary to bring K to [0,0,1]
//cos(K,K0) = [Kx,Ky,Kz].[0,0,1] = Kz
float Kxy = sqrt(K[0]*K[0] + K[1]*K[1]);
float anglePitch = - (pitch_roll * asin(K[0]/Kxy))*50;//*180/PI ;
float angleRoll = (pitch_roll * asin(K[1]/Kxy))*50;//*180/PI;
//float angleRoll = (atan2(dcmEst[2][2],dcmEst[2][1]))*180/3.14;
//float anglePitch = -(asin(dcmEst[2][0]))*180/3.14;
// float angleYaw=(-atan2(dcmGyro[1][0],dcmGyro[0][0]))*180/3.14;
float driftX=(sin(anglePitch*(3.14/180))*(sin(angleRoll*(3.14/180))));
float CalcDriftX=getAcclXOutput()-driftX;
float Acclx=getAcclXOutput();
float Gyrox=getGyroXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gyroy=getGyroYOutput();
//printf("\n");
//if(0 == imu_sequence % 4){
// hdlc_send_byte(float_to_int(anglePitch));
// hdlc_send_byte(float_to_int(angleRoll));
// hdlc_send_byte(float_to_int(Acclx*100));
// hdlc_send_byte(float_to_int(Gyrox*1000));
// hdlc_send_byte(float_to_int(Accly*100));
// hdlc_send_byte(float_to_int(Gyroy*1000));
// //hdlc_send_byte(float_to_int(Acclz*100));
// //hdlc_send_byte(float_to_int(Kxy*100));
// //hdlc_send_byte(float_to_int(K[0]*100));
// //hdlc_send_byte(float_to_int(K[1]*100));
// //hdlc_send_byte(float_to_int(K[2]*100));
//
//// float GyroYpitch = GyroYpitch+ (getGyroYOutput()*(interval_us/1000));
//// printf("pitch %1.3f roll %1.3f Gyro ",pitch.value,roll.value,GyroYpitch);
//// printf(" Angle P%1.3f R%1.3f Drift=%1.3f Acclx=%1.3f Calc=%1.3f GyroX=%1.3f\n",anglePitch,angleRoll,driftX,Acclx,CalcDriftX,GyroX);
// hdlc_send_sep();
//}
}
}
int threads_linux_init(void)
{
int status = 0;
int n = 0;
int return_value = THREADS_LINUX_SUCCESS;
// Init data
status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = gps_init(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = imu_init(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = magnetometer_init(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = sonar_init(&sonar_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = control_init(&control_data);
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = protocol_init();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = ui_init();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = datalogger_init();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
// Main Loop
if(return_value != THREADS_LINUX_FAILURE)
{
threads_linux_timer_start_task_1();
usleep(0.5*task_1_period_us);
threads_linux_timer_start_task_2();
usleep(5*task_1_period_us);
while(quittask == 0)
{
#if ANU_COMPILE_FOR_OVERO
#else
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
#endif
if(++n>100)
{
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
n = 0;
}
usleep(5*task_1_period_us);
}
threads_linux_timer_stop_task_1();
usleep(TASK1_PERIOD_US);
threads_linux_timer_stop_task_2();
usleep(TASK2_PERIOD_US);
}
status = datalogger_close();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = ui_close();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = protocol_close();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = control_close();
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = estimation_close(&estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = magnetometer_close(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = imu_close(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = gps_close(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = sensors_close();
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
return return_value;
}
void aos_init(int traj_nb)
{
aos.traj = &traj[traj_nb];
aos.time = 0;
aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY;
aos.traj->ts = 0;
aos.traj->ts = 1.; // default to one second
/* default state */
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_VECT3_ZERO(aos.ltp_pos);
FLOAT_VECT3_ZERO(aos.ltp_vel);
FLOAT_VECT3_ZERO(aos.ltp_accel);
FLOAT_VECT3_ZERO(aos.ltp_jerk);
aos.traj->init_fun();
imu_init();
ahrs_init();
#ifdef PERFECT_SENSORS
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
aos.heading_noise = 0.;
#else
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
aos.heading_noise = RadOfDeg(3.);
#endif
#ifdef FORCE_ALIGNEMENT
// DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
aos_compute_sensors();
// DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
// DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
// DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
ahrs_align();
// DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);
#endif
#ifdef DISABLE_ALIGNEMENT
printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
printf("# DISABLE_MAG_UPDATE\n");
#endif
printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]);
printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#if AHRS_MAG_UPDATE_YAW_ONLY
printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
printf("# PERFECT_SENSORS\n");
#endif
#if AHRS_USE_GPS_HEADING
printf("# AHRS_USE_GPS_HEADING\n");
#endif
#if USE_AHRS_GPS_ACCELERATIONS
printf("# USE_AHRS_GPS_ACCELERATIONS\n");
#endif
printf("# tajectory : %s\n", aos.traj->name);
};
application_t *application_init(struct config *cfg)
{
DEBUG("application_init()");
assert(cfg != 0);
application_t *app = calloc(1, sizeof(struct _application));
assert(app != 0);
// Initialize graphics
if(!(app->graphics = graphics_init(cfg->app_window_id)))
{
ERROR("Cannot initialize graphics");
goto error;
}
// Create atlases
if(!(app->atlas1 = graphics_atlas_create(cfg->graphics_font_file, cfg->graphics_font_size_1)) ||
!(app->atlas2 = graphics_atlas_create(cfg->graphics_font_file, cfg->graphics_font_size_2)))
{
ERROR("Cannot create atlas");
goto error;
}
// Create image
if(!(app->image = graphics_image_create(app->graphics, cfg->video_width, cfg->video_height, cfg->video_format, ANCHOR_CENTER)))
{
ERROR("Cannot create image");
goto error;
}
// Create HUD
if(!(app->hud = graphics_hud_create(app->graphics, app->atlas1, cfg->graphics_font_color_1, cfg->graphics_font_size_1, cfg->video_hfov, cfg->video_vfov)))
{
ERROR("Cannot create HUD");
goto error;
}
// Initialize GPS
memcpy(&app->gps_config, &cfg->gps_conf, sizeof(struct gps_config));
app->gps_config.userdata = app;
app->gps_config.create_label = create_label_handler;
app->gps_config.delete_label = delete_label_handler;
if(!(app->gps = gps_init(cfg->gps_device, &app->gps_config)))
{
ERROR("Cannot initialize GPS");
goto error;
}
// Initialize IMU
memcpy(&app->imu_config, &cfg->imu_conf, sizeof(struct imu_config));
if(!(app->imu = imu_init(cfg->imu_device, &app->imu_config)))
{
ERROR("Cannot initialize IMU");
goto error;
}
// Open video
if(!(app->video = video_open(cfg->video_device, cfg->video_width, cfg->video_height, cfg->video_format, cfg->video_interlace)))
{
ERROR("Cannot open video device");
goto error;
}
// Copy arguments
app->video_width = cfg->video_width;
app->video_height = cfg->video_height;
app->window_width = cfg->window_width;
app->window_height = cfg->window_height;
app->video_hfov = cfg->video_hfov;
app->video_vfov = cfg->video_vfov;
app->visible_distance = cfg->app_landmark_vis_dist;
memcpy(app->label_color, cfg->graphics_font_color_2, 4);
return app;
error:
if(app->video) video_close(app->video);
if(app->gps) gps_free(app->gps);
if(app->imu) imu_free(app->imu);
if(app->image) graphics_drawable_free(app->image);
if(app->hud) graphics_hud_free(app->hud);
if(app->atlas1) graphics_atlas_free(app->atlas1);
if(app->atlas2) graphics_atlas_free(app->atlas2);
if(app->graphics) graphics_free(app->graphics);
free(app);
return NULL;
}
void gyro_offset_calibration()
{
uint8_t i;
int16_t prev_gyro[3];
int16_t gyro[3];
float gyro_offset[3];
int8_t tilt_detected = 0;
int16_t gyro_calibration_counter = GYRO_ITERATIONS;
// TODO: Implement following function in spike_328p_i2c
imu_set_dlpf();
// TODO: Possibly implement in a tick process
// wait 2 seconds
//_delay_ms(2000);
//LOG("wait 2 sec...\r\n");
delay_millis(2000);
while(gyro_calibration_counter > 0)
{
if(gyro_calibration_counter == GYRO_ITERATIONS)
{
// TODO: Possibly implement in a tick process
delay_millis(700);
// TODO: Implement following function in spike_328p_i2c
imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]);
for(i=0; i<3; i++)
{
gyro_offset[i] = 0;
prev_gyro[i] = gyro[i];
}
}
//LOG("imu_get_rotation:\r\n");
imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]);
for (i=0; i<3; i++)
{
if(abs(prev_gyro[i] - gyro[i]) > TOL)
{
tilt_detected++;
#ifdef GYRO_DEBUG
LOG("i=%d counter=%d diff=%d gyro[i]=%d, prev_gyro[i]=%d\r\n", i, gyro_calibration_counter, prev_gyro[i] - gyro[i], gyro[i], prev_gyro[i]);
#endif
break;
}
}
for (i=0; i<3; i++)
{
gyro_offset[i] += (float)gyro[i]/GYRO_ITERATIONS;
prev_gyro[i] = gyro[i];
}
gyro_calibration_counter--;
if(tilt_detected >= 1)
{
LOG("gyro calibration failed, retrying...\r\n");
gyro_calibration_counter = GYRO_ITERATIONS;
tilt_detected = 0;
}
}
// Put results into integer
config.gyro_offset_x = (int16_t) gyro_offset[0];
config.gyro_offset_y = (int16_t) gyro_offset[1];
config.gyro_offset_z = (int16_t) gyro_offset[2];
LOG("updating gyro calibration offsets...\r\n");
LOG("config.gyro_offset[x,y,z]: %ld\t%ld\t%ld\r\n", config.gyro_offset_x, config.gyro_offset_y, config.gyro_offset_z);
// TODO: Review
imu_init();
}
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 imu_reset()
{
mpu.reset();
imu_init();
}
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_INS
ins_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_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 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
#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
}
// * imu_poll_gyro ************************************************************
// * poll gyro machine *
// ****************************************************************************
void imu_poll_gyro(void)
{
signed long calctemp; // calculation temporary variable
float floattemp; // floating point temporary variable
//retval = imu_i2c_start_transaction(IMU_ADDR_GYRO, 0x0F, imu_buff, 1, true, imu_catch_gyro);
switch(gyro_machine_state)
{
case GYRO_MACHINE_START_I2C: // init I2C state
{
imu_init(); // initialize gyro interface
// start transactions to be caught by next state
txn_done = false; // transaction not done by default
imu_buff[0] = 0x0F; // load configuration
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_CTRL_REG1, imu_buff, 1, false, imu_catch_gyro);
// start sending first configuration
gyro_machine_state = GYRO_MACHINE_CONFIG_1; // go to first configuration state
}
break;
case GYRO_MACHINE_CONFIG_1: // configuration 1 - init reg 1
{
//imu_i2c_start_transaction(unsigned char dev_address, unsigned char reg_address, unsigned char* data, unsigned int data_byte_count, tBoolean dir_is_receive, imu_i2c_xfer_done_t done_callback)
if(txn_done) // previous config done, next config
{
txn_done = false; // transaction not done by default
imu_buff[0] = 0x20; // load configuration (FULL SCALE 2000degrees/second)
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_CTRL_REG4, imu_buff, 1, false, imu_catch_gyro);
// start sending first configuration
gyro_machine_state = GYRO_MACHINE_CONFIG_2; // go to 2nd configuration state
}
}
break;
case GYRO_MACHINE_CONFIG_2: // configuration 2 - init reg 4
{
if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point
{
txn_done = false; // transaction not done by default
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro);
// start asking for first read
avg_count = IMU_GYRO_AVERAGES; // load down-counter with how many averages to take
zero_point = 0; // clear any previous zero point
gyro_machine_state = GYRO_MACHINE_ZEROING; // go to zeroing state
}
}
break;
case GYRO_MACHINE_ZEROING: // stationary condition averaging
{
if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point
{
txn_done = false; // transaction not done by default
zero_point += ((signed long)(*(signed short*)imu_buff));
// accumulate previous rest value
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro);
// ask for next rest value
avg_count--; // another sample read
if(avg_count == 0)
{
zero_point /= ((signed long)IMU_GYRO_AVERAGES); // finish average calculation, now have zero point at rest, you can now move the gyro
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro);
// ask for next (possibly dynamic) value to begin integrating
heading_millidegrees = 0; // initialize heading to zero
gyro_machine_state = GYRO_MACHINE_RUNNING; // go to running (integrating) state now that we have the zero point
}
}
}
break;
case GYRO_MACHINE_RUNNING: // normal operation - integrating deltas
{
if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point
{
txn_done = false; // transaction not done by default
calctemp = ((signed long)(*(signed short*)imu_buff));
// grab 2B 2's complement sample
imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro);
// ask for next rest value
floattemp = (float)(calctemp-zero_point); // would be better to leave this value raw then convert later to avoid accumulating error but this is faster to implement for now, note subtract out DC offset
yaw_rate_millidps = (signed long)(floattemp * ((float)IMU_GYRO_YAW_RATE_CONVERT_FACTOR));
// update yaw rate
floattemp *= (float)IMU_GYRO_CONVERT_FACTOR; // multiply delta by conversion factor
heading_millidegrees += (signed long)floattemp; // accumulate/integrate sample
}
}
break;
default:
gyro_machine_state = GYRO_MACHINE_START_I2C; // get machine back on track, should never be here
break;
}
}
