/************************************************************************ * initialize_controllers() * setup of feedback controllers used in flight core ************************************************************************/ int initialize_controllers(){ cstate.roll_ctrl = generatePID( config.Droll_KP, config.Droll_KI, config.Droll_KD, .015, DT); cstate.pitch_ctrl = generatePID( config.Dpitch_KP, config.Dpitch_KI, config.Dpitch_KD, .015, DT); cstate.yaw_ctrl = generatePID( config.yaw_KP, config.yaw_KI, config.yaw_KD, .015, DT); zero_out_controller(); if(parse_mix_layout(config.rotors, config.rotor_layout)){ return -1; } return 0; }
/************************************************************************ * arm_controller() * - zero out the controller * - set the setpoint.armed_state to ARMED * - enable motors ************************************************************************/ int arm_controller(){ zero_out_controller(); setpoint.arm_state = ARMED; setGRN(HIGH); setRED(HIGH); return 0; }
/****************************************************************** * 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; }
/************************************************************************ * arm_controller() * - zero out the controller * - set the setpoint.armed_state to ARMED * - enable motors ************************************************************************/ int arm_controller(){ zero_out_controller(); setpoint.arm_state = ARMED; set_led(GREEN,HIGH); set_led(RED,HIGH); 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; }
/****************************************************************** * arm_controller() * - zero out the controller * - set the setpoint.armed_state to ARMED * - enable motors *******************************************************************/ int arm_controller(){ zero_out_controller(); setpoint.arm_state = ARMED; enable_motors(); 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; }