/****************************************************************** * on_mode_release() * toggle between position and angle modes if MiP is paused *******************************************************************/ int on_mode_release(){ // store whether or not the controller was armed int was_armed = setpoint.arm_state; // disarm the controller if necessary if(was_armed == ARMED){ disarm_controller(); } // toggle between position and angle modes if(setpoint.core_mode == POSITION){ setpoint.core_mode = ANGLE; printf("using core_mode = ANGLE\n"); } else { setpoint.core_mode = POSITION; printf("using core_mode = POSITION\n"); } // arm the controller if it was armed before swapping modes if(was_armed == ARMED){ zero_out_controller(); arm_controller(); } blink_green(); return 0; }
/************************************************************************ * 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; }
/************************************************************************ * flight_core() * Hardware Interrupt-Driven Flight Control Loop * - read sensor values * - estimate system state * - read setpoint from flight_stack * - if is position mode, calculate a new attitude setpoint * - otherwise use user attitude setpoint * - calculate and send ESC commands ************************************************************************/ int flight_core(){ // remember previous arm mode to detect transition from DISARMED static arm_state_t previous_arm_state; int i; // general purpose static float new_esc[8]; static float u[4]; // normalized roll, pitch, yaw, throttle, components /************************************************************************ * Begin control loop if there was a valid interrupt with new IMU data ************************************************************************/ if (mpu9150_read(&mpu) != 0){ return -1; } /*********************************************************************** * STATE_ESTIMATION * read sensors and compute the state regardless of if the controller * is ARMED or DISARMED ************************************************************************/ // collect new IMU roll/pitch data // positive roll right according to right hand rule // MPU9150 driver has incorrect minus sign on Y axis, correct for it here // positive pitch backwards according to right hand rule cstate.roll = -(mpu.fusedEuler[VEC3_Y] - cstate.imu_roll_err); cstate.pitch = mpu.fusedEuler[VEC3_X] - cstate.imu_pitch_err; // current roll/pitch/yaw rates straight from gyro // converted to rad/s with default FUll scale range // raw gyro matches sign on MPU9150 coordinate system, unlike Euler angle cstate.dRoll = mpu.rawGyro[VEC3_Y] * GYRO_FSR * DEGREE_TO_RAD / 32767.0; cstate.dPitch = mpu.rawGyro[VEC3_X] * GYRO_FSR * DEGREE_TO_RAD / 32767.0; cstate.dYaw = mpu.rawGyro[VEC3_Z] * GYRO_FSR * DEGREE_TO_RAD / 32767.0; // if this is the first loop since being armed, reset yaw trim if(previous_arm_state==DISARMED && setpoint.arm_state!=DISARMED){ cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z]; cstate.num_yaw_spins = 0; } float new_yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) \ + (cstate.num_yaw_spins*2*PI); // detect the crossover point at Z = +-PI if(new_yaw - cstate.last_yaw > 6){ cstate.num_yaw_spins -= 1; } else if(new_yaw - cstate.last_yaw < -6){ cstate.num_yaw_spins += 1; } // also reset yaw if the throttle stick is down to prevent drift while on the ground if (user_interface.throttle_stick < -0.95){ cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z]; cstate.num_yaw_spins=0; } // record new yaw compensating for full rotation cstate.last_yaw = cstate.yaw; cstate.yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) + (cstate.num_yaw_spins*2*PI); /*********************************************************************** * Control based on the robotics_library defined state variable * PAUSED: make sure the controller stays DISARMED * RUNNING: Normal operation of controller. * - check for tipover * - choose mode from setpoint.core_mode * - evaluate difference equation and check saturation * - actuate motors ************************************************************************/ switch (get_state()){ // make sure things are off if program is closing case EXITING: return 0; // if the controller is somehow still ARMED, disarm it case PAUSED: if(setpoint.arm_state==ARMED){ disarm_controller(); } break; // normal operating mode case RUNNING: // exit if the controller was not armed properly if(setpoint.arm_state==DISARMED){ /************************************************************************ * if disarmed, reset controllers and return ************************************************************************/ zero_out_controller(); goto END; } // check for a tipover if (fabs(cstate.roll) >config.tip_angle || fabs(cstate.pitch)>config.tip_angle) { disarm_controller(); printf("\n TIPOVER DETECTED \n"); goto END; } /************************************************************************ * manage the setpoints based on attitude or position mode ************************************************************************/ switch(setpoint.core_mode){ /************************************************************************ * in Position control mode, evaluate an outer loop controller to * change the attitude setpoint. Discard user attitude setpoints ************************************************************************/ case POSITION: // TODO: outer loop position controller break; /************************************************************************ * in attitude control mode, user has direct control over throttle * roll, and pitch angles. Absolute yaw setpoint gets updated at * user-commanded yaw_rate ************************************************************************/ case ATTITUDE: // only when flying, update the yaw setpoint if(setpoint.throttle > YAW_CUTOFF_TH){ setpoint.yaw += DT*setpoint.yaw_rate; } else{ setpoint.yaw = cstate.yaw; } break; default: break; //should never get here } /************************************************************************ * Finally run the attitude feedback controllers ************************************************************************/ /************************************************************************ * Roll & Pitch Controllers ************************************************************************/ float dRoll_setpoint = (setpoint.roll - cstate.roll) * config.roll_rate_per_rad; float dPitch_setpoint = (setpoint.pitch - cstate.pitch) * config.pitch_rate_per_rad; cstate.dRoll_err = dRoll_setpoint - cstate.dRoll; cstate.dPitch_err = dPitch_setpoint - cstate.dPitch; // // if last state was DISARMED, then errors will all be 0. // // make the previous error the same // if(previous_core_mode == DISARMED){ // preFillFilter(&cstate.roll_ctrl, cstate.dRoll_err); // preFillFilter(&cstate.pitch_ctrl, cstate.dPitch_err); // } // only run integrator if airborne // TODO: proper landing/takeoff detection if(u[3] > INT_CUTOFF_TH){ cstate.dRoll_err_integrator += cstate.dRoll_err * DT; cstate.dPitch_err_integrator += cstate.dPitch_err * DT; } marchFilter(&cstate.roll_ctrl, cstate.dRoll_err); marchFilter(&cstate.pitch_ctrl, cstate.dPitch_err); if(setpoint.throttle<0.1){ saturateFilter(&cstate.roll_ctrl, -LAND_SATURATION,LAND_SATURATION); saturateFilter(&cstate.pitch_ctrl, -LAND_SATURATION, LAND_SATURATION); } else{ saturateFilter(&cstate.roll_ctrl, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT); saturateFilter(&cstate.pitch_ctrl, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT); } u[0] = cstate.roll_ctrl.current_output; u[1] = cstate.pitch_ctrl.current_output; /************************************************************************ * Yaw Controller ************************************************************************/ cstate.yaw_err = setpoint.yaw - cstate.yaw; // only run integrator if airborne if(u[3] > INT_CUTOFF_TH){ cstate.yaw_err_integrator += cstate.yaw_err * DT; } marchFilter(&cstate.yaw_ctrl, cstate.yaw_err); if (user_interface.throttle_stick < -0.95){ zeroFilter(&cstate.yaw_ctrl); } else{ saturateFilter(&cstate.yaw_ctrl, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT); } u[2] = cstate.yaw_ctrl.current_output; /************************************************************************ * Throttle Controller ************************************************************************/ // compensate for roll/pitch angle to maintain Z thrust float throttle_compensation; throttle_compensation = 1 / cos(cstate.roll); throttle_compensation *= 1 / cos(cstate.pitch); float thr = setpoint.throttle*(MAX_THRUST_COMPONENT-config.idle_speed) + config.idle_speed; u[3] = throttle_compensation * thr; // saturate thrust component now, this will help prevent saturation // later once r,p,y are added saturate_number(&u[3], 0, MAX_THRUST_COMPONENT); /************************************************************************ * see mixing_matrixes.h for how the mixer works ************************************************************************/ if(mix_controls(u[0],u[1],u[2],u[3],&new_esc[0],config.rotors)<0){ printf("ERROR, mixing failed\n"); return -1; } /************************************************************************ * Prevent saturation under heavy vertical acceleration by reducing all * outputs evenly such that the largest doesn't exceed 1 ************************************************************************/ // find control output limits float largest_value = 0; float smallest_value = 1; for(i=0;i<config.rotors;i++){ if(new_esc[i]>largest_value){ largest_value = new_esc[i]; } if(new_esc[i]<smallest_value){ smallest_value=new_esc[i]; } } // if upper saturation would have occurred, reduce all outputs evenly if(largest_value>1){ for(i=0;i<config.rotors;i++){ float offset = largest_value - 1; new_esc[i]-=offset; } } /************************************************************************ * Send a servo pulse immediately at the end of the control loop. * Intended to update ESCs exactly once per control timestep * also record this action to cstate.new_esc_out[] for telemetry ************************************************************************/ // if this is the first time armed, make sure to send minimum // pulse width to prevent ESCs from going into calibration if(previous_arm_state == DISARMED){ for(i=0;i<config.rotors;i++){ send_servo_pulse_normalized(i+1,0); } } else{ for(i=0;i<config.rotors;i++){ if(new_esc[i]>1.0){ new_esc[i]=1.0; } else if(new_esc[i]<0){ new_esc[i]=0; } send_servo_pulse_normalized(i+1,new_esc[i]); cstate.esc_out[i] = new_esc[i]; cstate.control_u[i] = u[i]; } } // // pass new information to the log with add_to_buffer // // this only puts information in memory, doesn't // // write to disk immediately // if(config.enable_logging){ // new_log_entry.roll = cstate.roll; // new_log_entry.pitch = cstate.pitch; // new_log_entry.yaw = cstate.yaw; // new_log_entry.u_0 = cstate.u[0]; // new_log_entry.u_1 = cstate.u[1]; // new_log_entry.u_2 = cstate.u[2]; // new_log_entry.u_3 = cstate.u[3]; // new_log_entry.esc_1 = cstate.esc_out[0]; // new_log_entry.esc_2 = cstate.esc_out[1]; // new_log_entry.esc_3 = cstate.esc_out[2]; // new_log_entry.esc_4 = cstate.esc_out[3]; // new_log_entry.v_batt = cstate.v_batt; // add_to_buffer(new_log_entry); // } break; default: break; } // end of switch(get_state()) END: // allow quick jump to here to clean up the several switch statements //remember the last state to detect transition from DISARMED to ARMED previous_arm_state = setpoint.arm_state; return 0; }
/************************************************************************ * flight_stack() * Translates the flight mode and user controls from user_interface * into setpoints for the flight_core position and attitude controller * * If the core gets disarmed by another thread, flight_stack manages * recognizing the rearming sequence * * The flight_core only takes setpoint values for feedback control, ************************************************************************/ void* flight_stack(void* ptr){ int i; //flight_mode_t previous_flight_mode; // wait for IMU to settle disarm_controller(); usleep(1000000); usleep(1000000); usleep(500000); set_state(RUNNING); setpoint.core_mode = POSITION; //start in position control setRED(LOW); setGRN(HIGH); // run until state indicates thread should close while(1){ switch (get_state()){ case EXITING: return NULL; case PAUSED: // not much to do if paused! break; case RUNNING: // if the core got disarmed, wait for arming sequence if(setpoint.arm_state == DISARMED){ wait_for_arming_sequence(); // user may have pressed the pause button or shut down while waiting // check before continuing if(get_state()!=RUNNING){ break; } // read config each time it's picked up to recognize new // settings user may have changed // only actually reads from disk if the file was modified load_config(&config); initialize_controllers(); // // write a blank log entry to mark this time // log_blank_entry(); // wake ESCs up at minimum throttle to avoid calibration mode // flight_core also sends one minimum pulse at first when armed for(i=0; i<config.rotors; i++){ send_servo_pulse_normalized(i+1,0); } usleep(10000); arm_controller(); } // kill switches seem to be fine // switch behaviour based on user flight mode if(setpoint.arm_state == ARMED){ // shutdown core on kill switch if(user_interface.kill_switch == DISARMED){ //printf("stack disarmed controller\n"); disarm_controller(); break; } if(user_interface.input_mode==NONE){ // no input devices connected, keep things zero'd disarm_controller(); break; } else{ // decide how to change setpoint based on flight mode switch(user_interface.flight_mode){ // Raw attitude mode lets user control the inner attitude loop directly case USER_ATTITUDE: setpoint.core_mode = ATTITUDE; // translate throttle stick (-1,1) to throttle (0,1) setpoint.throttle = (user_interface.throttle_stick + 1.0)/2.0; // scale roll and pitch angle by max setpoint in rad setpoint.roll = user_interface.roll_stick * config.max_roll_setpoint; setpoint.pitch = user_interface.pitch_stick * config.max_pitch_setpoint; // scale yaw_rate by max yaw rate in rad/s setpoint.yaw_rate = user_interface.yaw_stick * config.max_yaw_rate; break; // emergency land just sets the throttle low for now // TODO: gently lower altitude till landing detected case EMERGENCY_LAND: setpoint.core_mode = ATTITUDE; setpoint.throttle = config.land_throttle; setpoint.roll = 0; setpoint.pitch = 0; setpoint.yaw_rate = 0; break; // TODO: other modes // case USER_LOITER: // break; // case USER_POSITION_CARTESIAN: // break; // case USER_POSITION_RADIAL: // break; // case TARGET_HOLD: // break; default: break; } } } default: break; }// end of switch(get_state()) // // record previous flight mode to detect changes // previous_flight_mode = user_interface.flight_mode; // run about as fast as the core itself usleep(1000000/SAMPLE_RATE_HZ); } return NULL; }
/****************************************************************** * balance_core() * discrete-time balance controller operated off IMU interrupt * Called at SAMPLE_RATE_HZ *******************************************************************/ int balance_core(){ // local variables only in memory scope of balance_core static int D1_saturation_counter = 0; float compensated_D1_output = 0; float dutyL = 0; float dutyR = 0; static log_entry_t new_log_entry; float output_scale; //battery voltage/nominal voltage // if an IMU packet read failed, ignore and just return // the mpu9150_read function may print it's own warnings if (mpu9150_read(&mpu) != 0){ return -1; } /************************************************************** * STATE_ESTIMATION * read sensors and compute the state regardless of if the * controller is ARMED or DISARMED ***************************************************************/ // angle theta is positive in the direction of forward tip // add mounting angle of BBB cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0]; cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle; cstate.current_theta = cstate.theta[0]; cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT; // collect encoder positions cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \ /(config.gearbox * config.encoder_res); cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \ /(config.gearbox * config.encoder_res); // log phi estimate // wheel angle is relative to body, // add theta body angle to get absolute wheel position cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0]; cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta; cstate.current_phi = cstate.phi[0]; cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT; // body turning estimation cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0]; cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \ * (config.wheel_radius/config.track_width); cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT; cstate.current_gamma = cstate.gamma[0]; // output scaling output_scale = cstate.vBatt/config.v_nominal; /************************************************************* * Control based on the robotics_library defined state variable * PAUSED: make sure the controller stays DISARMED * RUNNING: Normal operation of controller. * - check for tipover * - wait for MiP to be within config.start_angle of * upright * - choose mode from setpoint.core_mode * - evaluate difference equation and check saturation * - actuate motors ***************************************************************/ switch (get_state()){ // make sure things are off if program is closing case EXITING: disable_motors(); return 0; // if the controller is somehow still ARMED, disarm it case PAUSED: if(setpoint.arm_state==ARMED){ disarm_controller(); } break; // normal operating mode case RUNNING: // exit if the controller was not armed properly if(setpoint.arm_state==DISARMED){ return 0; } // check for a tipover before anything else if(fabs(cstate.current_theta)>config.tip_angle){ disarm_controller(); printf("tip detected \n"); break; } /********************************************************** * POSITION Phi controller * feedback control of wheel angle Phi by outputting a * reference theta body angle. This is controller D2 in * config ***********************************************************/ if(setpoint.core_mode == POSITION){ // move the position set points based on user input if(setpoint.phi_dot != 0.0){ //setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT; setpoint.phi += setpoint.phi_dot * DT; } // march the different equation terms for the input Phi Error // and the output theta reference angle cstate.ePhi[2] = cstate.ePhi[1]; cstate.ePhi[1] = cstate.ePhi[0]; cstate.ePhi[0] = setpoint.phi-cstate.current_phi; cstate.theta_ref[2] = cstate.theta_ref[1]; cstate.theta_ref[1] = cstate.theta_ref[0]; // evaluate D2 difference equation cstate.theta_ref[0] = config.K_D2*( \ config.numD2_2 * cstate.ePhi[2] \ + config.numD2_1 * cstate.ePhi[1] \ + config.numD2_0 * cstate.ePhi[0]) \ -(config.denD2_2 * cstate.theta_ref[2] \ + config.denD2_1 * cstate.theta_ref[1]); //check saturation of outer loop theta reference output signal saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max); setpoint.theta = cstate.theta_ref[0]; } // evaluate inner loop controller D1z cstate.eTheta[2] = cstate.eTheta[1]; cstate.eTheta[1] = cstate.eTheta[0]; cstate.eTheta[0] = setpoint.theta - cstate.current_theta; cstate.u[2] = cstate.u[1]; cstate.u[1] = cstate.u[0]; cstate.u[0] = \ config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \ + config.numD1_1 * cstate.eTheta[1] \ + config.numD1_2 * cstate.eTheta[2]) \ - (config.denD1_1 * cstate.u[1] \ + config.denD1_2 * cstate.u[2]); // check saturation of inner loop knowing that right after // this control will be scaled by battery voltage if(saturate_float(&cstate.u[0], -output_scale, output_scale)){ D1_saturation_counter ++; if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){ printf("inner loop controller saturated\n"); disarm_controller(); D1_saturation_counter = 0; break; } } else{ D1_saturation_counter = 0; } cstate.current_u = cstate.u[0]; // scale output to compensate for battery charge level compensated_D1_output = cstate.u[0] / output_scale; // // integrate the reference theta to correct for imbalance or sensor // // only if standing relatively still with zero phi reference // // to-do, wait for stillness for a time period before integrating // if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){ // state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT; // } //steering controller // move the controller set points based on user input setpoint.gamma += setpoint.gamma_dot * DT; cstate.egamma[1] = cstate.egamma[0]; cstate.egamma[0] = setpoint.gamma - cstate.current_gamma; cstate.duty_split = config.KP_steer*(cstate.egamma[0] \ +config.KD_steer*(cstate.egamma[0]-cstate.egamma[1])); // if the steering input would saturate a motor, reduce // the steering input to prevent compromising balance input if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){ if(cstate.duty_split > 0){ cstate.duty_split = 1-fabs(compensated_D1_output); } else cstate.duty_split = -(1-fabs(compensated_D1_output)); } // add D1 balance controller and steering control dutyL = compensated_D1_output - cstate.duty_split; dutyR = compensated_D1_output + cstate.duty_split; // send to motors // one motor is flipped on chassis so reverse duty to L set_motor(config.motor_channel_L,-dutyL); set_motor(config.motor_channel_R,dutyR); cstate.time_us = microsSinceEpoch(); // pass new information to the log with add_to_buffer // this only puts information in memory, doesn't // write to disk immediately if(config.enable_logging){ new_log_entry.time_us = cstate.time_us-core_start_time_us; new_log_entry.theta = cstate.current_theta; new_log_entry.theta_ref = setpoint.theta; new_log_entry.phi = cstate.current_phi; new_log_entry.u = cstate.current_u; add_to_buffer(new_log_entry); } // end of normal balancing routine // last_state will be updated beginning of next interrupt break; default: break; // nothing to do if UNINITIALIZED } return 0; }
/****************************************************************** * balance_stack * This is the medium between the user_interface and setpoint * structs. dsm2, bluetooth, and mavlink threads may be * unreliable and shouldn't touch the controller setpoint * directly. balance_stack and balance_core should be the only * things touching the controller setpoint. *******************************************************************/ void* balance_stack(void* ptr){ // wait for IMU to settle disarm_controller(); usleep(1000000); usleep(1000000); usleep(500000); set_state(RUNNING); setpoint.core_mode = POSITION; //start in position control set_led(RED,LOW); set_led(GREEN,HIGH); // exiting condition is checked inside the switch case instead while(1){ switch (get_state()){ case EXITING: return NULL; case PAUSED: // not much to do if paused! break; // when running, balance_stack checks if an input mode // like mavlink, DSM2, or bluetooth is enabled // and moves the controller setpoints corresponding to // user input and current controller mode case RUNNING: if(setpoint.arm_state==DISARMED){ // check if the user has picked MIP upright before starting again wait_for_starting_condition(); // user may have pressed the pause button or shut down while waiting // check before continuing if(get_state()!=RUNNING){ break; } // write a blank log entry to mark this time log_blank_entry(); // read config each time it's picked up to recognize new // settings user may have changed // only actually reads from disk if the file was modified load_config(&config); zero_out_controller(); arm_controller(); } if(user_interface.input_mode == NONE){ // no user input, just keep the controller setpoint at zero setpoint.theta = 0; setpoint.phi_dot = 0; setpoint.gamma_dot = 0; break; } else{ setpoint.core_mode=POSITION; // scale user input from -1 to 1 to // the minimum and maximum phi_dot, the rate of change of the // phi reference angle // leave setpoint.theta alone as it is set by the core itself // using the D2 position controller saturate_float(&user_interface.drive_stick,-1,1); saturate_float(&user_interface.turn_stick,-1,1); // use a small deadzone to prevent slow drifts in position if(fabs(user_interface.drive_stick)<0.03)setpoint.phi_dot = 0; else if(user_interface.drive_mode == NOVICE) \ setpoint.phi_dot = config.drive_rate_novice*user_interface.drive_stick; else setpoint.phi_dot = config.drive_rate_advanced*user_interface.drive_stick; if(fabs(user_interface.turn_stick)<0.03) setpoint.gamma_dot = 0; else if(user_interface.drive_mode == NOVICE) \ setpoint.gamma_dot = config.turn_rate_novice*user_interface.turn_stick; else setpoint.gamma_dot = config.turn_rate_advanced*user_interface.turn_stick; } break; // end of RUNNING case default: break; } // end of switch get_state() // run about as fast as the core itself usleep(1000000/SAMPLE_RATE_HZ); } return NULL; }