diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c index ee77a7623965aab68635540caea1587768a7e655..202e035b3740c87ec7513938bb853c62ddec5946 100644 --- a/quad/sw/modular_quad_pid/src/control_algorithm.c +++ b/quad/sw/modular_quad_pid/src/control_algorithm.c @@ -10,7 +10,8 @@ #include "control_algorithm.h" #include "communication.h" -#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees +#define ROLL_PITCH_MAX_ANGLE 1.5708 // 90 degrees +#define PWM_DIFF_BOUNDS 30000 int control_algorithm_init(parameter_t * parameter_struct) { @@ -232,8 +233,10 @@ memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6); // don't use the PID corrections if the throttle is less than about 10% of its range + // NOTE!!!!!!!!!!!!!!!!!!!!!! + // re-enable the check for AUTO_FLIGHT_MODE when autonomous is fully enabled if((user_input_struct->rc_commands[THROTTLE] > - 118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)) + 118000))// || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)) { if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE) @@ -284,23 +287,23 @@ raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0; //BOUNDS CHECKING - if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000) - raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000; + if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[ROLL] = PWM_DIFF_BOUNDS; - if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000) - raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000; + if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -PWM_DIFF_BOUNDS; - if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000) - raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000; + if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[PITCH] = PWM_DIFF_BOUNDS; - if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000) - raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000; + if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -PWM_DIFF_BOUNDS; - if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000) - raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000; + if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[YAW] = PWM_DIFF_BOUNDS; - if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000) - raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000; + if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -PWM_DIFF_BOUNDS) + raw_actuator_struct->controller_corrected_motor_commands[YAW] = -PWM_DIFF_BOUNDS; } else diff --git a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c b/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c index d3773f2715d4916f226f8c1846cbe23d0dc22efd..75a63cc90ea57d28a08b9e5ca057c833ec88c4e4 100644 --- a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c +++ b/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c @@ -65,7 +65,7 @@ int startMPU9150(){ // Set clock reference to Z Gyro iic0Write(0x6B, 0x03); // Configure Digital Low/High Pass filter - iic0Write(0x1A,0x06); // Level 4 low pass on gyroscope + iic0Write(0x1A,0x06); // Level 6 low pass on gyroscope // Configure Gyro to 2000dps, Accel. to +/-8G iic0Write(0x1B, 0x18); diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c index 2ab248cdda53c460527bf294e95a0091e5a47151..f49808bfe41b08122194e8e41055bb6a7a19733e 100644 --- a/quad/sw/modular_quad_pid/src/log_data.c +++ b/quad/sw/modular_quad_pid/src/log_data.c @@ -143,7 +143,7 @@ void printLogging(){ ); - strcat(buf,comments); + //strcat(buf,comments); strcat(buf,header); strcat(buf,units);