/***********************************************************************
* main()
* start all the threads, and wait still something
* triggers a shut down
***********************************************************************/
int main(){
// initialize cape hardware
if(initialize_cape()<0){
blink_red();
return -1;
}
setRED(HIGH);
setGRN(LOW);
set_state(UNINITIALIZED);
// set up button handlers first
// so user can exit by holding pause
set_pause_pressed_func(&on_pause_press);
set_mode_unpressed_func(&on_mode_release);
// load data from disk.
if(load_config(&config)==-1){
printf("aborting, config file error\n");
return -1;
}
// start a thread to slowly sample battery
pthread_t battery_thread;
pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout))){
pthread_t printf_thread;
pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
}
// start listening for RC control from dsm2 radio
if(config.enable_dsm2){
if(initialize_dsm2()<0){
printf("failed to start DSM2\n");
}
else{
pthread_t dsm2_thread;
pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL);
}
}
// this thread is in charge of arming and managing the core
pthread_t drive_stack_thread;
pthread_create(&drive_stack_thread, NULL, drive_stack, (void*) NULL);
// all threads have started, off we go
set_state(RUNNING);
setRED(LOW);
setGRN(HIGH);
// chill until something exits the program
while(get_state()!=EXITING){
usleep(100000);
}
cleanup_cape(); // always end with cleanup to shut down cleanly
return 0;
}
/*
Response to a push in the planar directions.
*/
void MoveState::enter(Navdata n, Twist f){
std::cout << "Entering MOVE state" << std::endl;
blink_red();
start = Time::now();
steadyFlightStart = start;
steady = false;
}
void blink(const int n, const color_t c)
{
switch (c) {
case RED: blink_red(n); break;
case GREEN: blink_green(n); break;
case BLUE: blink_blue(n); break;
default: break;
}
}
static void
abort_handler (char *msg, char *file, int line)
{
USO_log_printf (USO_LL_PANIC, "\n ABORT: %s - %s - [%s, %d] !\n",
msg, USO_thread_name (), file, line);
for (;;){
blink_red (10);
USO_sleep (USO_MSEC_2_TICKS (RED_PAUSE/1000));
}
}
static void
panic_handler (char *msg, char *file, int line)
{
USO_disable ();
DEV_at91_DBGU_print_ascii ("\nPANIC: ");
DEV_at91_DBGU_print_ascii (msg);
DEV_at91_DBGU_print_ascii (" - ");
DEV_at91_DBGU_print_ascii (USO_thread_name ());
DEV_at91_DBGU_print_ascii (" - [");
DEV_at91_DBGU_print_ascii (file);
DEV_at91_DBGU_print_ascii ("] !\n");
for (;;){
blink_red (20);
DEV_cpudelay (DEV_USEC_2_LOOPS (RED_PAUSE));
}
}
/************************************************************************
* on_pause_press
* If the user holds the pause button for a second, exit cleanly
* disarm on momentary press
************************************************************************/
int on_pause_press(){
int i=0;
const int samples = 100; // check for release 100 times in this period
const int us_wait = 2000000; // 2 seconds
switch(get_state()){
// pause if running
case EXITING:
return 0;
case RUNNING:
set_state(PAUSED);
disarm_controller();
setRED(HIGH);
setGRN(LOW);
break;
case PAUSED:
set_state(RUNNING);
disarm_controller();
setGRN(HIGH);
setRED(LOW);
break;
default:
break;
}
// now wait to see if the user wants to shut down the program
while(i<samples){
usleep(us_wait/samples);
if(get_pause_button_state() == UNPRESSED){
return 0; //user let go before time-out
}
i++;
}
printf("long press detected, shutting down\n");
//user held the button down long enough, blink and exit cleanly
blink_red();
set_state(EXITING);
return 0;
}
/***********************************************************************
* main()
* initialize the IMU, start all the threads, and wait still something
* triggers a shut down
***********************************************************************/
int main(int argc, char* argv[]){
// initialize cape hardware
if(initialize_cape()<0){
blink_red();
return -1;
}
setRED(HIGH);
setGRN(LOW);
set_state(UNINITIALIZED);
// set up button handlers first
// so user can exit by holding pause
set_pause_pressed_func(&on_pause_press);
set_mode_unpressed_func(&on_mode_release);
// load data from disk.
if(load_config(&config)==-1){
printf("aborting, config file error\n");
return -1;
}
// start a thread to slowly sample battery
pthread_t battery_thread;
pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
// start listening for RC control from dsm2 radio
if(config.enable_dsm2){
if(initialize_dsm2()<0){
printf("failed to start DSM2\n");
}
else{
pthread_t dsm2_thread;
pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL);
}
}
// // start logging thread if enabled
// if(config.enable_logging){
// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
// printf("failed to start log\n");
// }
// else{
// // start new thread to write the file occationally
// pthread_t logging_thread;
// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
// }
// }
// // first check for user options
// if(parse_arguments(argc, argv)<0){
// return -1;
// }
// // start logging thread if enabled
// if(config.enable_logging){
// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
// printf("failed to start log\n");
// }
// else{
// // start new thread to write the file occationally
// pthread_t logging_thread;
// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
// }
// }
// // Start Safety checking thread
// pthread_create(&safety_thread, NULL, safety_thread_func, (void*) NULL);
// Finally start the real-time interrupt driven control thread
// start IMU with equilibrium set with upright orientation
// for MiP with Ethernet pointing relatively up
signed char orientation[9] = ORIENTATION_FLAT;
if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
// can't talk to IMU, all hope is lost
// blink red until the user exits
printf("IMU initialization failed, please reboot\n");
blink_red();
cleanup_cape();
return -1;
}
// assigning the interrupt function and stack
// should be the last step in initialization
// to make sure other setup functions don't interfere
printf("starting core IMU interrupt\n");
cstate.core_start_time_us = microsSinceEpoch();
set_imu_interrupt_func(&flight_core);
// start flight stack to control setpoints
// this thread is in charge of arming and managing the core
pthread_t flight_stack_thread;
pthread_create(&flight_stack_thread, NULL, flight_stack, (void*) NULL);
printf("\nReady for arming sequence\n");
set_state(RUNNING);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout))){
pthread_t printf_thread;
pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
}
//chill until something exits the program
while(get_state()!=EXITING){
usleep(100000);
}
// cleanup before closing
//close(sock); // mavlink UDP socket
cleanup_cape(); // de-initialize cape hardware
return 0;
}
/******************************************************************
* main()
* initialize the IMU, start all the threads, and wait still
* something triggers a shut down
*******************************************************************/
int main(){
initialize_cape();
set_led(RED,HIGH);
set_led(GREEN,LOW);
set_state(UNINITIALIZED);
// set up button handlers first
// so user can exit by holding pause
set_pause_pressed_func(&on_pause_press);
set_mode_unpressed_func(&on_mode_release);
// load data from disk.
if(load_config(&config)==-1){
printf("aborting, config file error\n");
return -1;
}
// start a thread to slowly sample battery
pthread_t battery_thread;
pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout))){
pthread_t printf_thread;
pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
}
// start listening for RC control from dsm2 radio
if(config.enable_dsm2){
initialize_dsm2();
pthread_t dsm2_thread;
pthread_create(&dsm2_thread, NULL, dsm2_listener, (void*) NULL);
}
// start mavlink if enabled
if(config.enable_mavlink_listening || config.enable_mavlink_listening){
char target_ip[16];
strcpy(target_ip, DEFAULT_MAV_ADDRESS);
// open a udp port for mavlink
// sock and gcAddr are global variables needed to send and receive
gcAddr = initialize_mavlink_udp(target_ip, udp_sock);
if(udp_sock != NULL){
printf("WARNING: continuing without mavlink enabled\n");
}
else {
if(config.enable_mavlink_listening){
// start a thread listening for incoming packets
pthread_t mav_listen_thread;
pthread_create(&mav_listen_thread, NULL, mavlink_listener, (void*) NULL);
printf("Listening for Packets\n");
}
if(config.enable_mavlink_transmitting){
// Start thread sending heartbeat and IMU attitude packets
pthread_t mav_send_thread;
pthread_create(&mav_send_thread, NULL, mavlink_sender, (void*) NULL);
printf("Transmitting Heartbeat Packets\n");
}
}
}
// start logging thread if enabled
if(config.enable_logging){
if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
printf("failed to start log\n");
}
else{
// start new thread to write the file occationally
pthread_t logging_thread;
pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
}
}
// Finally start the real-time interrupt driven control thread
// start IMU with equilibrium set with upright orientation
// for MiP with Ethernet pointing relatively up
signed char orientation[9] = ORIENTATION_UPRIGHT;
if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
// can't talk to IMU, all hope is lost
// blink red until the user exits
blink_red();
return -1;
}
// this should be the last step in initialization
// to make sure other setup functions don't interfere
printf("starting core IMU interrupt\n");
core_start_time_us = microsSinceEpoch();
set_imu_interrupt_func(&balance_core);
// start balance stack to control setpoints
pthread_t balance_stack_thread;
pthread_create(&balance_stack_thread, NULL, balance_stack, (void*) NULL);
printf("\nHold your MIP upright to begin balancing\n");
set_state(RUNNING);
// chill until something exits the program
while(get_state()!=EXITING){
usleep(100000);
}
// close(*udp_sock); // close network socket
cleanup_cape(); // always end with cleanup to shut down cleanly
return 0;
}
static void
idle_run (void)
{
/* System initialization without kernel logging!
* The scheduler is initialized and we are running on the idle
* thread so now we are allowed to start threads!
* The idle thread is not allowed to block!
*/
USO_buf_pool_init (&ios_buf_pool, ios_bufs, IOS_BUF_COUNT, IOS_BUF_SIZE);
ACE_stdio_init (&ios_buf_pool);
USO_sleep_init ();
DEV_timers_init ();
DEV_at91_FLASHD_initialize (SAM_MCK);
SAM_uart_init_0 ();
SAM_uart_init_1 ();
SAM_rtc_init ();
SAM_ticks_init ();
SAM_sys_interrupt_init ();
SAM_adc_init ();
SAM_pwm_init ();
SAM_events_init ();
blink_green (1);
ser0 = MFS_resolve("/sys/dev/serial/ser0");
if (ser0 == NULL)
{
ACE_PANIC ("Open ser0 fail");
}
/* Initialize kernel logging */
CLI_tty_init (&tty_0,
ser0,
CLI_TTY_INTRANSL_CR_2_NL,
CLI_TTY_OUTTRANSL_ADD_CR,
"tty0",
TRUE);
tty0 = MFS_resolve("/sys/cli/tty0");
if (tty0 == NULL)
{
ACE_PANIC ("Open tty0 fail");
}
CLI_tty_init (&tty_1,
ser0,
CLI_TTY_INTRANSL_CR_2_NL,
CLI_TTY_OUTTRANSL_ADD_CR,
"tty1",
FALSE);
CLI_switch_init();
CLI_switch_set(0,tty0);
CLI_tty_select(tty0);
ser1 = MFS_resolve("/sys/dev/serial/ser1");
if (ser1 == NULL)
{
ACE_PANIC ("Open ser1 fail");
}
/* The idle thread is not allowed to block
* so it is not allowed to do any printf or log output!!!!
*/
USO_log_init (tty0, USO_LL_INFO);
blink_green (1);
/* Interrupts must be enabled before starting the first thread! */
USO_enable ();
blink_green (1);
CLI_config_init ();
SAM_config_install ();
if (SAM_spi_init () != 0)
{
ACE_PANIC ("Spi init fail");
}
else
{
if (SAM_mmc_install () == ACE_OK)
{
CLI_config_read ();
} else {
blink_red(2);
}
}
/* ttyS0 is stdio for start thread,
* all other threads started(derived) from start thread
* will also use ttyS0 as stdio as long they don't change it.
*/
SAM_start (tty0);
/* idle */
}
/*
Response to a linear push in the planar directions.
*/
void FloatState::enter(Navdata n, Twist f){
std::cout << "Entering FLOAT state" << std::endl;
blink_red();
}
