|
|
|
|
|
// desired angle in |
|
|
// motor commands out (in degrees) |
|
|
void init_pids() |
|
|
{ |
|
|
max_stabilize_dampener = pid_stabilize_roll.kP() * 2500; |
|
|
stabilze_dampener = 5729.57795 * stabilize_rate_roll_pitch; |
|
|
|
|
|
max_yaw_dampener = pid_yaw.kP() * 6000; // .3 * 6000 = 1800 |
|
|
stabilze_yaw_dampener = 5729.57795 * stabilize_rate_yaw; // .3 |
|
|
} |
|
|
|
|
|
void output_stabilize() |
|
|
{ |
|
|
float roll_error, pitch_error; |
|
|
int max_out; |
|
|
Vector3f omega = dcm.get_gyro(); |
|
|
|
|
|
/*testing code:*/ |
|
|
//pitch_sensor = roll_sensor = 0; // testing only |
|
|
//stabilize_rate_roll_pitch = (float)rc_6.control_in / 1000; |
|
|
//init_pids(); |
|
|
|
|
|
// control +- 45° is mixed with the navigation request by the Autopilot |
|
|
// output is in degrees = target pitch and roll of copter |
|
|
rc_1.servo_out = rc_1.control_mix(nav_roll); |
|
|
rc_2.servo_out = rc_2.control_mix(nav_pitch); |
|
|
|
|
|
roll_error = rc_1.servo_out - roll_sensor; |
|
|
pitch_error = rc_2.servo_out - pitch_sensor; |
|
|
yaw_error = nav_yaw - yaw_sensor; |
|
|
yaw_error = wrap_180(yaw_error); |
|
|
|
|
|
// limit the error we're feeding to the PID |
|
|
roll_error = constrain(roll_error, -2500, 2500); |
|
|
pitch_error = constrain(pitch_error, -2500, 2500); |
|
|
yaw_error = constrain(yaw_error, -6000, 6000); |
|
|
|
|
|
//Serial.printf("s: %d \t mix %d, err %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error); |
|
|
|
|
|
// write out angles back to servo out - this will be converted to PWM by RC_Channel |
|
|
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, deltaMiliSeconds, 1.0); |
|
|
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0); |
|
|
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .3 = 198pwm |
|
|
|
|
|
//Serial.printf("\tpid: %d", (int)rc_1.servo_out); |
|
|
|
|
|
// We adjust the output by the rate of rotation: |
|
|
// Rate control through bias corrected gyro rates |
|
|
// omega is the raw gyro reading |
|
|
int roll_dampener = (omega.x * stabilze_dampener);// Omega is in radians |
|
|
int pitch_dampener = (omega.y * stabilze_dampener); |
|
|
int yaw_dampener = (omega.z * stabilze_yaw_dampener); |
|
|
|
|
|
// Limit dampening to be equal to propotional term for symmetry |
|
|
rc_1.servo_out -= constrain(roll_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15° |
|
|
rc_2.servo_out -= constrain(pitch_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15° |
|
|
rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener); // +- 15° |
|
|
|
|
|
//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener); |
|
|
|
|
|
//Serial.printf("\trd: %d", roll_dampener); |
|
|
//Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm()); |
|
|
} |
|
|
|
|
|
// err -2500 pid: -1100 rd: 1117 limit: -1650, PWM: -152 |
|
|
//s: -1247 mix 0, err 1247 pid: 548 rd: -153 limit: 395, PWM: 35 |
|
|
|
|
|
void output_rate_control() |
|
|
{ |
|
|
Vector3f omega = dcm.get_gyro(); |
|
|
|
|
|
rc_4.servo_out = rc_4.control_in; |
|
|
rc_1.servo_out = rc_2.control_in; |
|
|
rc_2.servo_out = rc_2.control_in; |
|
|
|
|
|
// Rate control through bias corrected gyro rates |
|
|
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected |
|
|
rc_1.servo_out -= (omega.x * 5729.57795 * acro_rate_roll_pitch); |
|
|
rc_2.servo_out -= (omega.y * 5729.57795 * acro_rate_roll_pitch); |
|
|
rc_4.servo_out -= (omega.z * 5729.57795 * acro_rate_yaw); |
|
|
|
|
|
//Serial.printf("\trated out %d, omega ", rc_1.servo_out); |
|
|
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3); |
|
|
|
|
|
// Limit output |
|
|
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); |
|
|
rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); |
|
|
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); |
|
|
} |
|
|
|
|
|
|
|
|
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. |
|
|
// Keeps outdated data out of our calculations |
|
|
void reset_I(void) |
|
|
{ |
|
|
pid_nav.reset_I(); |
|
|
pid_throttle.reset_I(); |
|
|
} |
|
|
|
|
|
|
|
|
/***************************************** |
|
|
* Set the flight control servos based on the current calculated values |
|
|
*****************************************/ |
|
|
void set_servos_4(void) |
|
|
{ |
|
|
static byte num; |
|
|
//motor_armed = false; |
|
|
// Quadcopter mix |
|
|
if (motor_armed == true) { |
|
|
int out_min = rc_3.radio_min; |
|
|
|
|
|
// Throttle is 0 to 1000 only |
|
|
rc_3.servo_out = constrain(rc_3.servo_out, 0, 1000); |
|
|
|
|
|
if(rc_3.servo_out > 0) |
|
|
out_min = rc_3.radio_min + 50; |
|
|
|
|
|
//Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); |
|
|
|
|
|
// creates the radio_out anf pwm_out values |
|
|
rc_1.calc_pwm(); |
|
|
rc_2.calc_pwm(); |
|
|
rc_3.calc_pwm(); |
|
|
rc_4.calc_pwm(); |
|
|
|
|
|
//Serial.printf("out: %d %d %d %d\n", rc_1.radio_out, rc_2.radio_out, rc_3.radio_out, rc_4.radio_out); |
|
|
//Serial.printf("yaw: %d ", rc_4.radio_out); |
|
|
|
|
|
if(frame_type == PLUS_FRAME){ |
|
|
motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out; |
|
|
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; |
|
|
motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out; |
|
|
motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; |
|
|
}else{ |
|
|
int roll_out = rc_1.pwm_out / 2; |
|
|
int pitch_out = rc_2.pwm_out / 2; |
|
|
motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; |
|
|
motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out; |
|
|
motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; |
|
|
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; |
|
|
} |
|
|
//Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); |
|
|
|
|
|
motor_out[RIGHT] += rc_4.pwm_out; |
|
|
motor_out[LEFT] += rc_4.pwm_out; |
|
|
motor_out[FRONT] -= rc_4.pwm_out; |
|
|
motor_out[BACK] -= rc_4.pwm_out; |
|
|
|
|
|
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); |
|
|
|
|
|
motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max); |
|
|
motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max); |
|
|
motor_out[FRONT] = constrain(motor_out[FRONT], out_min, rc_3.radio_max); |
|
|
motor_out[BACK] = constrain(motor_out[BACK], out_min, rc_3.radio_max); |
|
|
|
|
|
///* |
|
|
int r_out = ((long)(motor_out[RIGHT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); |
|
|
int l_out = ((long)(motor_out[LEFT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); |
|
|
int f_out = ((long)(motor_out[FRONT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); |
|
|
int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); |
|
|
//*/ |
|
|
|
|
|
//~#*set_servos_4: 398, -39 38 38 -36 |
|
|
/* |
|
|
num++; |
|
|
if (num > 50){ |
|
|
num = 0; |
|
|
Serial.printf("control_in: %d ", rc_3.control_in); |
|
|
Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); |
|
|
Serial.printf(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out); |
|
|
} |
|
|
//*/ |
|
|
|
|
|
//Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); |
|
|
//Serial.printf("s: %d %d %d\t\t", (int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor); |
|
|
///Serial.printf("outmin: %d\n", out_min); |
|
|
|
|
|
/* |
|
|
write_int(r_out); |
|
|
write_int(l_out); |
|
|
write_int(f_out); |
|
|
write_int(b_out); |
|
|
write_int((int)(roll_sensor / 100)); |
|
|
write_int((int)(pitch_sensor / 100)); |
|
|
write_int((int)(yaw_sensor / 100)); |
|
|
write_int((int)(yaw_error / 100)); |
|
|
write_int((int)(current_loc.alt)); |
|
|
write_int((int)(altitude_error)); |
|
|
flush(10); |
|
|
//*/ |
|
|
|
|
|
// Send commands to motors |
|
|
if(rc_3.servo_out > 0){ |
|
|
APM_RC.OutputCh(CH_1, motor_out[RIGHT]); |
|
|
APM_RC.OutputCh(CH_2, motor_out[LEFT]); |
|
|
APM_RC.OutputCh(CH_3, motor_out[FRONT]); |
|
|
APM_RC.OutputCh(CH_4, motor_out[BACK]); |
|
|
}else{ |
|
|
APM_RC.OutputCh(CH_1, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_2, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_3, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_4, rc_3.radio_min); |
|
|
} |
|
|
|
|
|
// InstantPWM |
|
|
APM_RC.Force_Out0_Out1(); |
|
|
APM_RC.Force_Out2_Out3(); |
|
|
|
|
|
}else{ |
|
|
|
|
|
// Send commands to motors |
|
|
APM_RC.OutputCh(CH_1, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_2, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_3, rc_3.radio_min); |
|
|
APM_RC.OutputCh(CH_4, rc_3.radio_min); |
|
|
|
|
|
// reset I terms of PID controls |
|
|
reset_I(); |
|
|
|
|
|
// Initialize yaw command to actual yaw when throttle is down... |
|
|
rc_4.control_in = ToDeg(yaw); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
void demo_servos(byte i) { |
|
|
// nothing to do |
|
|
} |
|
|
|
|
|
|
