void task_GPS(void) {
user_debug_msg(DBG_MSG, STR_TASK_GPS ": Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
while (1) {
gps_update();
//Display current latitude, longitude, velocity and heading
#if 1
sprintf(str_tmp, STR_TASK_GPS ": %09.2f,%9.4f,%10.4f,%6.1f,%6.1f,%4.1f,%7.3f,%7.2f,%7.2f,%u,%u,%u,%2X,%u,%u,%u,%u",
gps_read().time,gps_read().latitude,gps_read().longitude,gps_read().altitude,gps_read().geoid,
gps_read().hdop,gps_read().speed,gps_read().heading,gps_read().mag_var,gps_read().num_sat,
gps_read().stationID,gps_read().date,gps_read().fixflag,
gps_read().rmc_update,gps_read().gga_update,gps_read().rmc_count,gps_read().gga_count);
user_debug_msg(DBG_MSG, str_tmp);
#endif
// Note that this is not in keeping with all the other TAPS -- this should be driven by TAP_delay, etc.
OS_Delay(500);
}
}
static inline void main_init( void ) {
mcu_init();
sys_time_init();
led_init();
gps_init();
mcu_int_enable();
}
bool engineInitSystem()
{
engine_shutdown();
if (!engine_init()) {
//
}
initMapData();
#if defined(ARM_LINUX)
loadGrammar();
registerCallback();
#endif
g_navDisplay->initContext();
g_navDisplay->initViewport();
g_navDisplay->initDisplay(SCREEN_WIDTH,SCREEN_HEIGHT);
LatLongGrid_Init(&g_latLongGrid);
LatLongGrid_SetStep(&g_latLongGrid, 100000, 100000);
gps_init();
#if defined(ARM_LINUX)
gps_setContext(NMEA_CONTEXT_GPS);
#else
gps_setContext(NMEA_CONTEXT_MANUAL);
#endif
}
int gpsd_main(int argc, char *argv[])
{
// print text
printf("GPS deamon ready\n\n");
int ret = gps_init();
fflush(stdout);
// sigset_t sigset;
// sigemptyset(&sigset);
// sigaddset(SI_USER, &sigset);
struct sigaction act;
act.sa_u._sa_handler = interrupt;
// act.sa_mask = sigset;
sigaction(SI_USER, &act, NULL);
while(true)
{
char ch;
/* blocking read on next byte */
read (fd, &ch, 1);
printf("%c", ch);
}
/* Should never reach here, only on error */
return ret;
}
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
}
int main( void )
{
uint8_t init_cpt;
/* init peripherals */
timer_init();
modem_init();
adc_init();
#ifdef CTL_BRD_V1_1
adc_buf_channel(ADC_CHANNEL_BAT, &buf_bat);
#endif
spi_init();
link_fbw_init();
gps_init();
nav_init();
ir_init();
estimator_init();
# ifdef PAPABENCH_SINGLE
fbw_init();
# endif
/* start interrupt task */
//sei(); /*Fadia*/
/* Wait 0.5s (for modem init ?) */
init_cpt = 30;
_Pragma("loopbound min 31 max 31")
while (init_cpt) {
if (timer_periodic())
init_cpt--;
}
/* enter mainloop */
#ifndef NO_MAINLOOP
while( 1 ) {
#endif
if(timer_periodic()) {
periodic_task();
# if PAPABENCH_SINGLE
fbw_schedule();
# endif
}
if (gps_msg_received)
{
/*receive_gps_data_task()*/
parse_gps_msg();
send_gps_pos();
send_radIR();
send_takeOff();
}
if (link_fbw_receive_complete) {
link_fbw_receive_complete = FALSE;
radio_control_task();
}
#ifndef NO_MAINLOOP
}
#endif
return 0;
}
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);
}
int main(void)
{
CLKPR = (1<<7);
CLKPR = 0;
/* Disable watchdog timer */
MCUSR = 0;
wdt_disable();
/* Start the multi-threading kernel */
init_kernel(STACK_MAIN);
/* Timer */
TCCR2B = 0x03; /* Pre-scaler for timer0 */
TCCR2A = 1<<WGM21; /* CTC mode */
TIMSK2 = 1<<OCIE2A; /* Interrupt on compare match */
OCR2A = (SCALED_F_CPU / 32 / 2400) - 1;
TRACE_INIT;
sei();
t_stackErrorHandler(stackOverflow);
fbuf_errorHandler(bufferOverflow);
reset_params();
/* HDLC and AFSK setup */
mon_init(&cdc_outstr);
adf7021_init();
inframes = hdlc_init_decoder( afsk_init_decoder() );
outframes = hdlc_init_encoder( afsk_init_encoder() );
digipeater_init();
/* Activate digipeater in ui thread */
/* USB */
usb_init();
THREAD_START(usbSerListener, STACK_USBLISTENER);
ui_init();
/* GPS and tracking */
gps_init(&cdc_outstr);
tracker_init();
TRACE(1);
while(1)
{
lbeep();
if (t_is_idle()) {
/* Enter idle mode or sleep mode here */
powerdown_handler();
sleep_mode();
}
else
t_yield();
}
}
int main(int argc, char** argv)
{
// keep these values available for later
mp_argc = argc;
mp_argv = argv;
#else
int main(void)
{
mcu_init();
#endif
#if (USE_TELELOG == 1)
log_init();
#endif
#if (USE_USB == 1)
preflight(); // perhaps this would be better called usb_init()
#endif
gps_init(); // this sets function pointers so i'm calling it early for now
udb_init();
dcm_init();
init_config(); // this will need to be moved up in order to support runtime hardware options
#if (FLIGHT_PLAN_TYPE == FP_WAYPOINTS)
init_waypoints();
#endif
init_servoPrepare();
init_states();
init_behavior();
init_serial();
if (setjmp(buf))
{
// a processor exception occurred and we're resuming execution here
DPRINT("longjmp'd\r\n");
}
#ifdef _MSC_VER
#if (SERIAL_OUTPUT_FORMAT == SERIAL_MAVLINK)
parameter_table_init();
#endif // SERIAL_OUTPUT_FORMAT
#endif // _MSC_VER
while (1)
{
#if (USE_TELELOG == 1)
telemetry_log();
#endif
#if (USE_USB == 1)
USBPollingService();
#endif
#if (CONSOLE_UART != 0 && SILSIM == 0)
console();
#endif
udb_run();
}
return 0;
}
static inline void main_init( void ) {
hw_init();
sys_time_init();
led_init();
comm_init(COMM_TELEMETRY);
gps_init();
int_enable();
}
int main(void) {
avr_init();
serial_init();
gps_init();
// gps_on_receive_position(new_position_handler);
printf("boot and init completed.\nentering tracking loop...\n");
while(1) { gps_receive(); }
return(0);
}
bool initialise()
{
bool success = true;
USART_init(USART_PC,9600);
debug_println("Begininning Initialisation...");
initTimers();
if(gps_demonstration==true)
{
//GPS Demonstration
altimeter_init();
init_HMC5883L();
gps_init();
//Initialise a series of waypoints in a circle around the current coordinates..
_delay_ms(1000);
chris_waypoint_init();
}
else
{
//RX and servo Demonstration
rx_init();
quad_output_init();
}
debug_println("Initialization succeeded!");
//beep some pattern I can recognize
debug_beep_long();
_delay_ms(250);
debug_beep();
_delay_ms(250);
debug_beep();
_delay_ms(250);
debug_beep();
_delay_ms(250);
debug_beep_long();
return success;
}
void init_ap( void ) {
OSCCAL=0xB1;
uart1_init_rx();
uart0_init_rx();
/** adc_init done in fbw */
adc_buf_channel(ADC_CHANNEL_RANGEMETER, &rangemeter_adc_buf, DEFAULT_AV_NB_SAMPLE);
gps_init ();
int_enable();
/** Debug */
SetBit(DDRD, 5);
}
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();
}
int main(int argc, char *argv[])
{
portnum = 0;
if (argc > 1) {
portnum = atoi(argv[1]);
if (portnum > 10) {
printf("Port number must be <11\n");
return -1;
}
if (argc > 2) { configgps = atoi(argv[2]); }
if (configgps)
#ifdef GPS_CONFIGURE
printf("Will configure GPS.\n");
#else
printf("Rebuild with GPS configure support.\n");
#endif
}
printf("Using /dev/ttyUSB%d for GPS\n", portnum);
(void) signal(SIGINT, main_exit);
uart_init();
gps_init();
/* Initalize the event library */
event_init();
gcs_com_init();
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return -1;
}
#ifdef GPS_CONFIGURE
//periodic task is launched so we are now ready to use uart to request gps baud change...
if (configgps) { gps_configure_uart(); }
#endif
event_dispatch();
//should never occur!
printf("goodbye! (%d)\n", foo);
return 0;
}
int main()
{
gps_init();
log_init();
radio_init();
sms_init();
statusled_init();
timer1_init();
timer3_init();
watchdog_init();
/* Interrupts on - go go go! */
sei();
/* Now sleep - the whole program is interrupt driven */
for (;;) sleep_mode();
}
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();
}
main()
{
struct Location pos, dest;
//Test values, at the Geisel end of Warren Mall.
dest.latitude = 32.881136091348665;
dest.longitude = -117.23563715815544;
//Uncomment to spoof NMEA Strings on stdio
//tty = 0;
while(gps_init() < 1)
{}
while (1)
{
if (gps_get_position(&pos) < 1)
printf("Data inValid.\n");
printf("From %f,%f to %f,%f: %f at heading %f\n",pos.latitude,pos.longitude,dest.latitude,dest.longitude,calc_target_distance(&pos,&dest),calc_target_heading(&pos,&dest));
}
}
void vPapabenchInit()
{
mode = MODE_AUTO;
pprz_mode = PPRZ_MODE_HOME;
timer_init();
modem_init();
adc_init();
#ifdef CTL_BRD_V1_1
adc_buf_channel(uint8_t adc_channel, struct adc_buf *s);
#endif
spi_init();
link_fbw_init();
gps_init();
nav_init();
ir_init();
estimator_init();
#ifdef PAPABENCH_SINGLE
fbw_init();
#endif
}
int main(void) {
//uwrite_init();
gps_init();
// It looks like the uwrite and gps libraries cannot play
// nicely together on the same USART port. Switch to the Mega.
//uwrite_print_buff("Here\r\n");
while (1) {
//statevars.status = 0;
//gps_update();
//uwrite_print_buff("statevars.status: ");
//uwrite_print_long(&statevars.status);
}
return 0;
}
void fc_init()
{
DEBUG(" *** Flight computer init ***\n");
//start values
active_page = config.gui.last_page;
if (active_page >= config.gui.number_of_pages)
active_page = 0;
//reset flight status
fc_reset();
//using fake data
#ifdef FAKE_ENABLE
return;
#endif
//temperature state machine
fc.temp.step = 0;
//init DMA
DMA_PWR_ON;
//init calculators
vario_init();
audio_init();
logger_init();
protocol_init();
wind_init();
gps_init();
if (config.connectivity.use_gps)
gps_start();
bt_init();
if (config.connectivity.use_bt)
bt_module_init();
//VCC to baro, acc/mag gyro + i2c pull-ups
mems_power_on();
//init and test i2c
//HW_REW_1504 have two mems enable pins, both have to be enabled!
//HW_REW_1506 have standalone ldo for mems, hence only one pin is needed
if (!mems_i2c_init())
{
DEBUG("ERROR I2C, Wrong board rev? (%02X)\n", hw_revision);
hw_revision = HW_REW_1504;
eeprom_busy_wait();
eeprom_update_byte(&config_ro.hw_revision, hw_revision);
eeprom_busy_wait();
mems_power_init();
io_init();
mems_power_on();
assert(mems_i2c_init());
}
else
{
if (hw_revision == HW_REW_UNKNOWN)
{
hw_revision = HW_REW_1506;
eeprom_busy_wait();
eeprom_update_byte(&config_ro.hw_revision, hw_revision);
eeprom_busy_wait();
mems_power_init();
io_init();
mems_power_on();
mems_i2c_init();
}
}
if (!mems_i2c_init())
{
DEBUG("I2C error!\nUnable to init flight computer!\n");
return;
}
//Barometer
ms5611.Init(&mems_i2c, MS5611_ADDRESS_CSB_LO);
//Magnetometer + Accelerometer
lsm303d_settings lsm_cfg;
lsm_cfg.enabled = true;
lsm_cfg.accOdr = lsm_acc_1600Hz;
lsm_cfg.accScale = lsm_acc_16g;
lsm_cfg.magOdr = lsm_mag_100Hz;
lsm_cfg.magScale = lsm_mag_4g;
lsm_cfg.magHiRes = true;
lsm_cfg.tempEnable = false;
//Acceleration calculation init
accel_calc_init();
//Magnetic field calculation init
mag_calc_init();
//Gyro
l3gd20_settings l3g_cfg;
l3g_cfg.enabled = true;
l3g_cfg.bw = l3g_50Hz;
l3g_cfg.odr = l3g_760Hz;
l3g_cfg.scale = l3g_2000dps;
//SHT21
sht21_settings sht_cfg;
sht_cfg.rh_enabled = true;
sht_cfg.temp_enabled = true;
//XXX: do self-test?
lsm303d.Init(&mems_i2c, lsm_cfg);
lsm303d.Start();
l3gd20.Init(&mems_i2c, l3g_cfg);
l3gd20.Start();
sht21.Init(&mems_i2c, sht_cfg);
//Measurement timer
FC_MEAS_TIMER_PWR_ON;
fc_meas_timer.Init(FC_MEAS_TIMER, timer_div1024);
fc_meas_timer.SetInterruptPriority(MEDIUM);
fc_meas_timer.EnableInterrupts(timer_overflow | timer_compareA | timer_compareB | timer_compareC);
//tight timing! 1 tick 0.032 ms
//MS pressure conversion 9.040 ms
// temperature conversion 0.600 ms
//MAG read 0.152 ms
//ACC read 1.600 ms
//Gyro read 1.000 ms
fc_meas_timer.SetTop(313); // == 10ms
fc_meas_timer.SetCompare(timer_A, 27); // == 0.78 ms
fc_meas_timer.SetCompare(timer_B, 70); // == 2 ms
fc_meas_timer.SetCompare(timer_C, 200); // == 6 ms
ms5611.StartTemperature();
lsm303d.StartReadMag(); //it takes 152us to transfer
_delay_ms(1);
fc_meas_timer.Start();
DEBUG(" *** FC init done ***\n");
}
int main()
{
// Disable, configure, and start the watchdog timer
wdt_disable();
wdt_reset();
wdt_enable(WDTO_8S);
// Start and configure all hardware peripherals
sei();
led_init();
radio_init();
gps_init();
radio_enable();
// Set the radio shift and baud rate
_radio_dac_write(RADIO_COARSE, RADIO_CENTER_FREQ);
_radio_dac_write(RADIO_FINE, 0);
radio_set_shift(RADIO_SHIFT_425);
radio_set_baud(RADIO_BAUD_50);
// Radio chatter
for(uint8_t i = 0; i < 5; i++)
{
radio_chatter();
wdt_reset();
}
int32_t lat = 0, lon = 0, alt = 0;
uint8_t hour = 0, minute = 0, second = 0, lock = 0, sats = 0;
while(true)
{
led_set(LED_GREEN, 1);
// Get the current system tick and increment
uint32_t tick = eeprom_read_dword(&ticks) + 1;
// Check that we're in airborne <1g mode
if( gps_check_nav() != 0x06 ) led_set(LED_RED, 1);
// Get information from the GPS
gps_check_lock(&lock, &sats);
if( lock == 0x02 || lock == 0x03 || lock == 0x04 )
{
gps_get_position(&lat, &lon, &alt);
gps_get_time(&hour, &minute, &second);
}
led_set(LED_GREEN, 0);
// Format the telemetry string & transmit
double lat_fmt = (double)lat / 10000000.0;
double lon_fmt = (double)lon / 10000000.0;
alt /= 1000;
sprintf(s, "$$" CALLSIGN ",%lu,%02u:%02u:%02u,%02.7f,%03.7f,%ld,%u,%x",
tick, hour, minute, second, lat_fmt, lon_fmt, alt,
sats, lock);
radio_chatter();
radio_transmit_sentence(s);
radio_chatter();
led_set(LED_RED, 0);
eeprom_update_dword(&ticks, tick);
wdt_reset();
_delay_ms(500);
}
return 0;
}
/******************************************************************************
**** ****
** **
task_monitor()
This task proves the proper functioning of the passthrough feature of the
GPSRM 1. Passthrough connects the OEM615V's COM1 port to one of three pairs
of IO lines on the CSK bus: IO.[5,4], IO.[17,16] or IO.[33,32]. Which pair
is implemented depends on the GPSRM 1's Assembly Revision (ASSY REV).
task_gps() initially configures the GPSRM 1 with passthrough enabled, but
without any logging active in the OEM615V.
task_monitor() (re-)starts once commanded to by task_gps() ... this happens
after all the basic tests (I2C working, can power OEM615V on and off) are
concluded, the OEM615V is on and not in reset, and is ready to talk via COM1.
task_monitor() then commands the OEM615V to start logging, and then scans
the resulting NMEA strings from the OEM615V for valid GPS fix and GPS data.
** **
**** ****
******************************************************************************/
void task_monitor(void) {
// Initially we're stopped, waiting for task_gps to configure the GPSRM1 and OEM615V.
user_debug_msg(STR_TASK_MONITOR "Stopped.");
OS_Stop();
// Eventually task_gps() starts this task.
user_debug_msg(STR_TASK_MONITOR "Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
// Now that we're ready for NMEA messages from the OEM615V, tell it to
// start logging them at 1Hz on its COM1.
// We need to send this out to all three possible paths into the GPSRM 1's
// passthrough port.
// Don't forget to terminate the string with CRLF!
csk_uart1_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart2_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart3_puts("LOG COM1 GPGGA ONTIME 1\r\n");
// Additionally, tell the OEM615V to output a pulsetrain (2ms @ 125kHz) via the VARF output
csk_uart1_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart2_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart3_puts("FREQUENCYOUT ENABLE 200 800\r\n");
// Wait a few seconds for the NMEA buffers to fill ...
OS_Delay(200);
OS_Delay(200);
// We remain here forever, parsing NMEA strings from the OEM615 for GPS status
// and data. While this is happening (and after a GPS fix has been achieved),
// it's a good time to test disconnecting and reconnecting the GPS antenna from
// the OEM615V, to see that its GPS Position Valid LED goes out when the
// antenna is disconnected.
while(1) {
// Update gps values from incoming NMEA strings.
gps_update();
// Display current sats in view, HDOP, altitude, latitude & longitude if we have a fix.
if((gps_read().fixflag&gps_fix)||(gps_read().fixflag&diffgps_fix)) {
sprintf(strTmp, STR_TASK_MONITOR " GMT Sat HDOP Alt. Latitude Longitude\r\n" \
"\t\t\t\t-----------------------------------------------\r\n" \
"\t\t\t\t%s %02u %3.1f %6.1fm %8.4f %8.4f\r\n",
gps_NMEA_GMT_time_hhmmss(), gps_read().num_sat, gps_read().hdop, gps_read().altitude, gps_read().latitude, gps_read().longitude);
} /* if() */
// O/wise indicate that we do not have a fix.
else {
sprintf(strTmp, STR_TASK_MONITOR "No valid GPS position -- check antenna.\r\n");
} /* else() */
user_debug_msg(strTmp);
// Repeat every 5s.
OS_Delay(250);
OS_Delay(250);
} /* while() */
} /* task_monitor() */
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;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Function: main
// Description: Application entry point.
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int main(void)
{
// Set system clock source to 16 MHz with the PLL
clocks_16mhz_pll();
// Configure GPIO's for SPI, UART, ADC, IO, etc.
gpio_init();
i2c_init();
while(1);
// Configure spi bus for SD Card
spi_init(SD_SPI, SPI_CPOL1_CPHA1);
// Configure spi bus for external ADC
spi_init(SPI2, SPI_CPOL0_CPHA1);
// Configure GPS module and usart1
gps_init();
// Initialize ads1255
ads1255_init();
#if USEFATFS
// Mount drive, stop if it doesn't work
if (f_mount(&DriveSDCard, "1", 1)) goto FAIL;
#else
// Init sequence to use sd card in spi mode
if (sd_init() != SDR_Success) goto FAIL;
sd_readCSD();
// Trap execution if you can't read CSD register correctly
if (SDCard.CardBlockSize == 0) goto FAIL;
#endif
#if LOGGING
k = 0; // Monitors buffer size
block = 15000; // Starting block of SD card to write to
DEBUG_TOGGLE_A5(); // Toggles LED off to show that startup was successfull
// Main app loop
if(rx_buffer[3]!='G') goto FAIL; // Failed gps initialization, restart required
while (!gpsHasFix); // Wait to get a gps fix to set the internal time
ms = gps_getTimeInMs() + 40; // Synchronize initial time to gps time, with potential 40 ms delay accounted for
while (1)
{
conv = ads1255_singleConversion(); // Get conversion from adc
// Write 32 byte packet to buffer every second
if((ms%1000) == 0)
{
time = gps_getTimeInMs();
gpsLat = gps_getLatitudeInt();
gpsLong = gps_getLongitudeInt();
gpsAlt = gps_getAltitudeInt();
k = addToBuffer(buffer, k, startStopByte, 1);
k = addToBuffer(buffer, k, ms, 4);
k = addToBuffer(buffer, k, conv, 3);
k = addToBuffer(buffer, k, time, 4);
k = addToBuffer(buffer, k, gpsLat, 4);
k = addToBuffer(buffer, k, gpsLong,4);
k = addToBuffer(buffer, k, gpsAlt, 3);
k = addToBuffer(buffer, k, wind, 2);
k = addToBuffer(buffer, k, temperature, 2);
checksum = ms + conv + time + gpsLat + gpsLong + gpsAlt + wind + temperature;
k = addToBuffer(buffer, k, checksum, 4);
ms += 10;
if(k>503) // Checking for buffer size
doWrite = 1;
}
else // Write 8 byte packet to buffer for every adc read
{
k = addToBuffer(buffer, k, 0x5A,1);
k = addToBuffer(buffer, k, ms, 4);
k = addToBuffer(buffer, k, conv, 3);
ms += 10;
if(~(ms%1000) == 0)
{
if(k>480)
{
doWrite = 1;
}
}
else if(k>503)
{
doWrite = 1;
}
}
if (doWrite)
{
sd_writeBlock(block++, &buffer[0], sizeof(buffer));
DEBUG_TOGGLE_A5();
k = 0;
doWrite = 0;
}
}
#else
#endif
// Something went wrong in init sequence, turn on bad LED and trap executuion
//------------------------------------------------------------------------------------
FAIL:
DEBUG_A5_HIGH();
while (1);
//------------------------------------------------------------------------------------
// How did I get here?
return 0;
}
void fc_init()
{
DEBUG(" *** Flight computer init ***\n");
//load configuration
cfg_load();
//start values
eeprom_busy_wait();
active_page = eeprom_read_byte(&config.gui.last_page);
fc.epoch_flight_start = 0;
fc.autostart_state = false;
fc.temp_step = 0;
//init calculators
vario_init();
audio_init();
gps_init();
if (fc.use_gps)
gps_start();
bt_init();
// if (fc.use_flage & ENABLE_BT)
// bt_module_init();
//VCC to baro, acc/mag gyro
MEMS_POWER_ON;
GpioSetDirection(IO0, OUTPUT);
GpioWrite(IO0, HIGH);
//init and test i2c
if (!mems_i2c_init())
{
DEBUG("ERROR I2C\n");
led_set(0xFF, 0, 0);
}
//Barometer
ms5611.Init(&mems_i2c, MS5611_ADDRESS_CSB_LO);
//Magnetometer + Accelerometer
lsm303d_settings lsm_cfg;
lsm_cfg.enabled = true;
lsm_cfg.accOdr = lsm_acc_1600Hz;
lsm_cfg.accScale = lsm_acc_16g;
lsm_cfg.magOdr = lsm_mag_100Hz;
lsm_cfg.magScale = lsm_mag_4g;
lsm_cfg.magHiRes = true;
lsm_cfg.tempEnable = false;
//Gyro
l3gd20_settings l3g_cfg;
l3g_cfg.enabled = true;
l3g_cfg.bw = l3g_50Hz;
l3g_cfg.odr = l3g_760Hz;
l3g_cfg.scale = l3g_2000dps;
sht21_settings sht_cfg;
sht_cfg.rh_enabled = true;
sht_cfg.temp_enabled = true;
//XXX: do self-test?
lsm303d.Init(&mems_i2c, lsm_cfg);
lsm303d.Start();
l3gd20.Init(&mems_i2c, l3g_cfg);
l3gd20.Start();
sht21.Init(&mems_i2c, sht_cfg);
//Measurement timer
FC_MEAS_TIMER_PWR_ON;
fc_meas_timer.Init(FC_MEAS_TIMER, timer_div256); //125 == 1ms
fc_meas_timer.SetInterruptPriority(MEDIUM);
fc_meas_timer.EnableInterrupts(timer_overflow | timer_compareA | timer_compareB | timer_compareC);
fc_meas_timer.SetTop(125 * 10); // == 10ms
fc_meas_timer.SetCompare(timer_A, 100); // == 0.64ms
fc_meas_timer.SetCompare(timer_B, 430); // == 2.7ms
fc_meas_timer.SetCompare(timer_C, 555); // == 3.7ms
fc_meas_timer.Start();
DEBUG(" *** FC init done ***\n");
}
