2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
/*************************************************************
|
|
|
|
throttle control
|
|
|
|
****************************************************************/
|
|
|
|
|
|
|
|
// user input:
|
|
|
|
// -----------
|
|
|
|
void output_manual_throttle()
|
|
|
|
{
|
|
|
|
rc_3.servo_out = rc_3.control_in;
|
2010-12-26 01:18:25 -04:00
|
|
|
rc_3.servo_out = (float)rc_3.servo_out * angle_boost();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Autopilot
|
|
|
|
// ---------
|
|
|
|
void output_auto_throttle()
|
|
|
|
{
|
2010-12-26 01:18:25 -04:00
|
|
|
rc_3.servo_out = (float)nav_throttle * angle_boost();
|
|
|
|
rc_3.servo_out = max(rc_3.servo_out, 1);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void calc_nav_throttle()
|
|
|
|
{
|
2011-01-09 22:24:26 -04:00
|
|
|
long t_out;
|
|
|
|
|
|
|
|
if(altitude_sensor == BARO) {
|
2011-01-17 22:48:44 -04:00
|
|
|
t_out = pid_baro_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
2011-01-09 22:24:26 -04:00
|
|
|
|
|
|
|
// limit output of throttle control
|
|
|
|
t_out = throttle_cruise + constrain(t_out, -50, 100);
|
|
|
|
} else {
|
|
|
|
// SONAR
|
2011-01-17 22:48:44 -04:00
|
|
|
t_out = pid_sonar_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
2011-01-09 22:24:26 -04:00
|
|
|
|
|
|
|
// limit output of throttle control
|
|
|
|
t_out = throttle_cruise + constrain(t_out, -60, 100);
|
|
|
|
}
|
|
|
|
|
|
|
|
nav_throttle = (float)(throttle_cruise + t_out) * angle_boost();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2010-12-26 01:18:25 -04:00
|
|
|
float angle_boost()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
|
|
|
//static byte flipper;
|
2011-01-09 22:24:26 -04:00
|
|
|
//float temp = 1 / (cos(dcm.roll) * cos(dcm.pitch));
|
|
|
|
float temp = (1.0 - (cos(dcm.roll) * cos(dcm.pitch)));
|
|
|
|
temp = 1.0 + (temp / 1.5);
|
|
|
|
|
|
|
|
// limit the boost value
|
|
|
|
if(temp > 1.3)
|
|
|
|
temp = 1.3;
|
2010-12-19 12:40:33 -04:00
|
|
|
return temp;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*************************************************************
|
|
|
|
yaw control
|
|
|
|
****************************************************************/
|
|
|
|
|
2011-01-02 16:34:42 -04:00
|
|
|
void output_manual_yaw()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
|
|
|
if(rc_3.control_in == 0){
|
2011-01-02 16:34:42 -04:00
|
|
|
clear_yaw_control();
|
|
|
|
} else {
|
|
|
|
// Yaw control
|
|
|
|
if(rc_4.control_in == 0){
|
|
|
|
//clear_yaw_control();
|
|
|
|
output_yaw_with_hold(true); // hold yaw
|
|
|
|
}else{
|
|
|
|
output_yaw_with_hold(false); // rate control yaw
|
|
|
|
}
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2011-01-02 16:34:42 -04:00
|
|
|
void auto_yaw()
|
2010-12-26 01:18:25 -04:00
|
|
|
{
|
2011-01-02 16:34:42 -04:00
|
|
|
output_yaw_with_hold(true); // hold yaw
|
2010-12-26 01:18:25 -04:00
|
|
|
}
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
/*************************************************************
|
|
|
|
picth and roll control
|
|
|
|
****************************************************************/
|
|
|
|
|
2011-01-11 17:12:29 -04:00
|
|
|
/*// how hard to tilt towards the target
|
2010-12-19 12:40:33 -04:00
|
|
|
// -----------------------------------
|
|
|
|
void calc_nav_pid()
|
|
|
|
{
|
2011-01-09 22:24:26 -04:00
|
|
|
// how hard to pitch to target
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
nav_angle = pid_nav.get_pid(wp_distance * 100, dTnav, 1.0);
|
|
|
|
nav_angle = constrain(nav_angle, -pitch_max, pitch_max);
|
|
|
|
}
|
|
|
|
|
|
|
|
// distribute the pitch angle based on our orientation
|
|
|
|
// ---------------------------------------------------
|
|
|
|
void calc_nav_pitch()
|
|
|
|
{
|
2011-01-09 22:24:26 -04:00
|
|
|
// how hard to pitch to target
|
|
|
|
|
|
|
|
long angle = wrap_360(nav_bearing - yaw_sensor);
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
bool rev = false;
|
|
|
|
float roll_out;
|
|
|
|
|
2011-01-09 22:24:26 -04:00
|
|
|
if(angle > 18000){
|
|
|
|
angle -= 18000;
|
2010-12-19 12:40:33 -04:00
|
|
|
rev = true;
|
|
|
|
}
|
|
|
|
|
2011-01-09 22:24:26 -04:00
|
|
|
roll_out = abs(angle - 18000);
|
2010-12-19 12:40:33 -04:00
|
|
|
roll_out = (9000.0 - roll_out) / 9000.0;
|
|
|
|
roll_out = (rev) ? roll_out : -roll_out;
|
|
|
|
|
|
|
|
nav_pitch = (float)nav_angle * roll_out;
|
|
|
|
}
|
|
|
|
|
|
|
|
// distribute the roll angle based on our orientation
|
|
|
|
// --------------------------------------------------
|
|
|
|
void calc_nav_roll()
|
|
|
|
{
|
2011-01-09 22:24:26 -04:00
|
|
|
long angle = wrap_360(nav_bearing - yaw_sensor);
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
bool rev = false;
|
|
|
|
float roll_out;
|
|
|
|
|
2011-01-09 22:24:26 -04:00
|
|
|
if(angle > 18000){
|
|
|
|
angle -= 18000;
|
2010-12-19 12:40:33 -04:00
|
|
|
rev = true;
|
|
|
|
}
|
|
|
|
|
2011-01-09 22:24:26 -04:00
|
|
|
roll_out = abs(angle - 9000);
|
2010-12-19 12:40:33 -04:00
|
|
|
roll_out = (9000.0 - roll_out) / 9000.0;
|
|
|
|
roll_out = (rev) ? -roll_out : roll_out;
|
|
|
|
|
|
|
|
nav_roll = (float)nav_angle * roll_out;
|
|
|
|
}
|
2011-01-11 17:12:29 -04:00
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-01-09 22:24:26 -04:00
|
|
|
|