static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
stateInit();
actuators_init();
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
settings_init();
mcu_int_enable();
downlink_init();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
int
main( void )
{
v_t angles;
uint32_t start;
uint32_t stop;
init_uart();
init_timer();
sbi( TIMSK, TOIE1 );
sei();
puts( "ahrs_init()\r\n" );
ahrs_init( 0, 0, 1, 0 );
while(1)
{
puts( "\r\nstep: " );
start = high_time();
ahrs_step( angles, 0.1, 0, 0, 0, 0, 0, 1, 0 );
stop = high_time();
put_uint16_t( (stop >> 16) & 0xFFFF );
put_uint16_t( (stop >> 0) & 0xFFFF );
puts( " - " );
put_uint16_t( (start >> 16) & 0xFFFF );
put_uint16_t( (start >> 0) & 0xFFFF );
stop -= start;
puts( " = " );
put_uint16_t( (stop >> 16) & 0xFFFF );
put_uint16_t( (stop >> 0) & 0xFFFF );
}
}
int main(int argc, char** argv) {
read_ascii_flight_log(IN_FILE);
imu_init();
ahrs_init();
for (int i=0; i<nb_samples; i++) {
feed_imu(i);
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
ahrs_propagate((1./AHRS_PROPAGATE_FREQUENCY));
#ifdef ENABLE_MAG_UPDATE
if (MAG_AVAILABLE(samples[i].flag))
ahrs_update_mag();
#endif
#ifdef ENABLE_ACCEL_UPDATE
if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag)))
ahrs_update_accel();
#endif
}
store_filter_output(i);
}
dump_output(OUT_FILE);
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_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
}
void run_ukf(void) {
/* initial state covariance matrix */
double P[7*7] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
/* model noise covariance matrix */
double Q[7*7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.008, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008 };
/* measurement noise covariance matrix */
double R[1]={0.3};
/* state [q0, q1, q2, q3, bp, bq, br] */
double X[7];
/* measure */
double Y[1];
/* command */
double U[3] = {0.0, 0.0, 0.0};
ukf_filter filter;
filter = ukf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
ahrs_init(ad, 150, X);
ukf_filter_reset(filter, X, P);
ukf_filter_compute_weights(filter, 1.1, 0.0, 2.0);
int iter;
for (iter=0; iter < ad->nb_samples; iter++) {
U[0] = ad->gyro_p[iter];// - X[4];
U[1] = ad->gyro_q[iter];// - X[5];
U[2] = ad->gyro_r[iter];// - X[6];
ahrs_state = iter%3;
switch (ahrs_state) {
case UPDATE_PHI:
Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
break;
case UPDATE_THETA:
Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
break;
case UPDATE_PSI: {
double eulers[3];
eulers_of_quat(eulers, X);
Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
break;
}
}
ukf_filter_update(filter, Y, U);
ukf_filter_get_state(filter, X, P);
ahrs_data_save_state(ad, iter, X, P);
}
ukf_filter_delete(filter);
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
// motor_mixing_init();
// servo_mixing_init();
set_servo_init();
#endif
radio_control_init();
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_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);
baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
downlink_init();
mcu_int_enable();
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
imu_init();
ahrs_aligner_init();
ahrs_init();
DEBUG_SERVO1_INIT();
// DEBUG_SERVO2_INIT();
mcu_int_enable();
}
static inline void main_init( void ) {
mcu_init();
sys_time_init();
imu_init();
ahrs_aligner_init();
ahrs_init();
DEBUG_SERVO1_INIT();
// DEBUG_SERVO2_INIT();
mcu_int_enable();
}
STATIC_INLINE void main_init( void ) {
#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
/* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
__asm("nop");
}
#endif
hw_init();
sys_time_init();
actuators_init();
radio_control_init();
booz2_analog_init();
baro_init();
#if defined USE_CAM || USE_DROP
booz2_pwm_init_hw();
#endif
battery_init();
imu_init();
// booz_fms_init(); // FIXME
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#ifdef USE_GPS
booz_gps_init();
#endif
modules_init();
int_enable();
}
STATIC_INLINE void main_init( void ) {
#ifndef NO_FUCKING_STARTUP_DELAY
#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
/* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
__asm("nop");
}
#endif
#endif
mcu_init();
sys_time_init();
electrical_init();
actuators_init();
radio_control_init();
#if DATALINK == XBEE
xbee_init();
#endif
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#ifdef USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
actuators_init();
radio_control_init();
#if DATALINK == XBEE
xbee_init();
#endif
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
baro_init();
ins_init();
#if USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_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);
baro_tid = sys_time_register_timer(0.02, NULL);
telemetry_tid = sys_time_register_timer((1./60.), NULL);
}
static inline void autopilot_main_init( void ) {
hw_init();
sys_time_init();
led_init();
supervision_init();
actuators_init(ACTUATOR_BANK_MOTORS);
#if BOMB_ENABLED
bomb_init_servo(RADIO_FMS, 0, 0);
#endif
rc_init();
#if HARDWARE_ENABLED_GPS
gps_init();
#else
comm_init(COMM_0);
comm_add_tx_callback(COMM_0, comm_autopilot_message_send);
comm_add_rx_callback(COMM_0, comm_autopilot_message_received);
#endif
comm_init(COMM_TELEMETRY);
comm_add_tx_callback(COMM_TELEMETRY, comm_autopilot_message_send);
comm_add_rx_callback(COMM_TELEMETRY, comm_autopilot_message_received);
analog_init();
analog_enable_channel(ANALOG_CHANNEL_BATTERY);
analog_enable_channel(ANALOG_CHANNEL_PRESSURE);
altimeter_init();
imu_init();
autopilot_init();
ahrs_init();
ins_init();
fms_init();
int_enable();
}
int main ( int argc, char** argv) {
printf("hello\n");
g_timeout_add(1000/25, timeout_callback, NULL);
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit ("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
imu_init();
ahrs_init();
aos_init();
g_main_loop_run(ml);
return 0;
}
static inline void main_init(void) {
hw_init();
sys_time_init();
imu_init();
baro_init();
radio_control_init();
actuators_init();
overo_link_init();
cscp_init();
adc_init();
#ifdef PASSTHROUGH_CYGNUS
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#endif
adc_buf_channel(0, &adc0_buf, 8);
adc_buf_channel(1, &adc1_buf, 8);
adc_buf_channel(2, &adc2_buf, 8);
adc_buf_channel(3, &adc3_buf, 8);
cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg);
new_radio_msg = FALSE;
new_baro_diff = FALSE;
new_baro_abs = FALSE;
new_vane = FALSE;
new_adc = FALSE;
overo_link.up.msg.imu_tick = 0;
}
int main(void)
{
SetupHardware();
data_init(&data, &calib_params); //put some values into structure for testing
ahrs_init(&data, &u8g, &calib_params);
GlobalInterruptEnable();
for (;;)
{
//time between updates
timer_old = timer;
timer = get_timer_ms();
if (timer > timer_old) {
t_period = (timer - timer_old) / 1000.0; //in seconds
}
else
t_period = 0;
data.time_period = t_period;
//read data - sensors
adxl345_read(&data); //accelerometer read
l3g4200d_read_seq(&data); //gyroscope read
hmc5883l_read(&data); //magnetometer read
//buttons
buttons_read(&data);
//compute
//ahrs_orientation_from_gyro(&data);
ahrs_orientation(&data);
//ahrs_orientation_from_accel_mag(&data);
HID_Task();
USB_USBTask();
}
}
int
main( void )
{
v_t angles;
ahrs_init( 0, 0, 1, 0 );
while(1)
{
ahrs_step( angles, 0.1, 0, 0, 0, 0, 0, 1, 0 );
printf( "Angles: %lf %lf %lf\n",
angles[0],
angles[1],
angles[2]
);
getchar();
}
return 0;
}
STATIC_INLINE void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
#ifndef INTER_MCU_AP
actuators_init();
#else
intermcu_init();
#endif
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
#ifndef INTER_MCU_AP
radio_control_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
/* temporary hack:
* Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
* This led to the problem that global waypoints were not "localized",
* so as a stop-gap measure we localize them all (again) here..
*/
waypoints_localize_all();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
#ifdef INTER_MCU_AP
intermcu_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
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
}
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();
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 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;
}
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);
};
void init_ap(void)
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/** - start interrupt task */
mcu_int_enable();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_AHRS
ahrs_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif
/************ Internal status ***************/
autopilot_init();
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
#if !USE_GENERATED_AUTOPILOT
navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
#endif
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);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if DOWNLINK
downlink_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
PPRZ_MUTEX_LOCK(ap_state_mtx);
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
syslog(LOG_INFO, "initializing core");
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("core") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("core.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
exit(EXIT_FAILURE);
}
syslog(LOG_CRIT, "logger opened");
sleep(1); /* give scl some time to establish
a link between publisher and subscriber */
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "| core startup |");
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "initializing system");
/* set-up real-time scheduling: */
struct sched_param sp;
sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
sched_setscheduler(getpid(), SCHED_FIFO, &sp);
if (mlockall(MCL_CURRENT | MCL_FUTURE))
{
LOG(LL_ERROR, "mlockall() failed");
exit(EXIT_FAILURE);
}
/* initialize hardware/drivers: */
omap_i2c_bus_init();
baro_altimeter_init();
ultra_altimeter_init();
ahrs_init();
motors_init();
voltage_reader_start();
//gps_init();
LOG(LL_INFO, "initializing model/controller");
model_init();
ctrl_init();
/* initialize command interface */
LOG(LL_INFO, "initializing cmd interface");
cmd_init();
/* prepare main loop: */
for (int i = 0; i < NUM_AVG; i++)
{
output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f);
}
LOG(LL_INFO, "system up and running");
struct timespec ts_curr;
struct timespec ts_prev;
struct timespec ts_diff;
clock_gettime(CLOCK_REALTIME, &ts_curr);
/* run model and controller: */
while (1)
{
/* calculate dt: */
ts_prev = ts_curr;
clock_gettime(CLOCK_REALTIME, &ts_curr);
TIMESPEC_SUB(ts_diff, ts_curr, ts_prev);
float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC;
/* read sensor values into model input structure: */
model_input_t model_input;
model_input.dt = dt;
ahrs_read(&model_input.ahrs_data);
gps_read(&model_input.gps_data);
model_input.ultra_z = ultra_altimeter_read();
model_input.baro_z = baro_altimeter_read();
/* execute model step: */
model_state_t model_state;
model_step(&model_state, &model_input);
/* execute controller step: */
mixer_in_t mixer_in;
ctrl_step(&mixer_in, dt, &model_state);
/* set up mixer input: */
mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch);
mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll);
mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw);
mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas);
/* write data to motor mixer: */
EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in));
}
}
