2012-04-30 04:17:14 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2012-11-27 20:42:22 -04:00
|
|
|
/*****************************************
|
|
|
|
* Throttle slew limit
|
|
|
|
*****************************************/
|
2012-11-27 21:13:39 -04:00
|
|
|
static void throttle_slew_limit(int16_t last_throttle)
|
2012-11-27 20:42:22 -04:00
|
|
|
{
|
|
|
|
// if slew limit rate is set to zero then do not slew limit
|
|
|
|
if (g.throttle_slewrate) {
|
|
|
|
// limit throttle change by the given percentage per second
|
2013-01-10 14:42:24 -04:00
|
|
|
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min);
|
2012-11-27 21:13:39 -04:00
|
|
|
// allow a minimum change of 1 PWM per cycle
|
|
|
|
if (temp < 1) {
|
|
|
|
temp = 1;
|
|
|
|
}
|
2012-12-18 22:35:13 -04:00
|
|
|
g.channel_throttle.radio_out = constrain_int16(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp);
|
2012-11-27 20:42:22 -04:00
|
|
|
}
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
/*
|
|
|
|
check for triggering of start of auto mode
|
|
|
|
*/
|
|
|
|
static bool auto_check_trigger(void)
|
|
|
|
{
|
|
|
|
// only applies to AUTO mode
|
|
|
|
if (control_mode != AUTO) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// if already triggered, then return true, so you don't
|
|
|
|
// need to hold the switch down
|
|
|
|
if (auto_triggered) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (g.auto_trigger_pin == -1 && g.auto_kickstart == 0.0f) {
|
|
|
|
// no trigger configured - let's go!
|
|
|
|
auto_triggered = true;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (g.auto_trigger_pin != -1) {
|
2013-03-21 22:31:35 -03:00
|
|
|
int8_t pin = hal.gpio->analogPinToDigitalPin(g.auto_trigger_pin);
|
|
|
|
if (pin != -1) {
|
|
|
|
// ensure we are in input mode
|
|
|
|
hal.gpio->pinMode(pin, GPIO_INPUT);
|
|
|
|
|
|
|
|
// enable pullup
|
|
|
|
hal.gpio->write(pin, 1);
|
|
|
|
|
|
|
|
if (hal.gpio->read(pin) == 0) {
|
|
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
|
|
|
|
auto_triggered = true;
|
|
|
|
return true;
|
|
|
|
}
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (g.auto_kickstart != 0.0f) {
|
2013-03-21 21:54:04 -03:00
|
|
|
float xaccel = ins.get_accel().x;
|
|
|
|
if (xaccel >= g.auto_kickstart) {
|
|
|
|
gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
|
2013-03-21 19:38:25 -03:00
|
|
|
auto_triggered = true;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
|
|
|
calculate the throtte for auto-throttle modes
|
|
|
|
*/
|
2013-03-01 07:32:57 -04:00
|
|
|
static void calc_throttle(float target_speed)
|
2012-12-18 00:42:11 -04:00
|
|
|
{
|
2013-03-21 19:38:25 -03:00
|
|
|
if (!auto_check_trigger()) {
|
|
|
|
g.channel_throttle.servo_out = g.throttle_min.get();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
if (target_speed <= 0) {
|
|
|
|
// cope with zero requested speed
|
|
|
|
g.channel_throttle.servo_out = g.throttle_min.get();
|
|
|
|
return;
|
|
|
|
}
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
int throttle_target = g.throttle_cruise + throttle_nudge;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
|
|
|
reduce target speed in proportion to turning rate, up to the
|
|
|
|
SPEED_TURN_GAIN percentage.
|
2013-03-01 07:32:57 -04:00
|
|
|
*/
|
2013-04-18 21:23:57 -03:00
|
|
|
float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX);
|
2013-05-01 21:26:12 -03:00
|
|
|
steer_rate = constrain_float(steer_rate, 0.0, 1.0);
|
2013-03-01 20:03:15 -04:00
|
|
|
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
|
|
|
|
|
|
|
|
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
|
|
|
|
// in auto-modes we reduce speed when approaching waypoints
|
|
|
|
float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
|
|
|
|
if (reduction2 < reduction) {
|
|
|
|
reduction = reduction2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// reduce the target speed by the reduction factor
|
|
|
|
target_speed *= reduction;
|
|
|
|
|
|
|
|
groundspeed_error = target_speed - ground_speed;
|
|
|
|
|
|
|
|
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
// also reduce the throttle by the reduction factor. This gives a
|
|
|
|
// much faster response in turns
|
|
|
|
throttle *= reduction;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
|
|
|
* Calculate desired turn angles (in medium freq loop)
|
|
|
|
*****************************************/
|
|
|
|
|
2013-02-07 18:21:22 -04:00
|
|
|
static void calc_nav_steer()
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
|
|
|
// Adjust gain based on ground speed
|
2013-03-28 19:14:58 -03:00
|
|
|
if (ground_speed < 0.01) {
|
|
|
|
nav_gain_scaler = 1.4f;
|
|
|
|
} else {
|
|
|
|
nav_gain_scaler = g.speed_cruise / ground_speed;
|
|
|
|
}
|
2013-05-01 21:26:12 -03:00
|
|
|
nav_gain_scaler = constrain_float(nav_gain_scaler, 0.2f, 1.4f);
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
// Calculate the required turn of the wheels rover
|
|
|
|
// ----------------------------------------
|
|
|
|
|
2012-11-29 03:09:05 -04:00
|
|
|
// negative error = left turn
|
2012-04-30 04:17:14 -03:00
|
|
|
// positive error = right turn
|
2013-04-18 21:23:57 -03:00
|
|
|
nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler);
|
2012-06-13 15:43:35 -03:00
|
|
|
|
2013-03-28 20:49:08 -03:00
|
|
|
// avoid obstacles, if any
|
2013-04-18 21:23:57 -03:00
|
|
|
nav_steer_cd += obstacle.turn_angle*100;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-04-18 21:23:57 -03:00
|
|
|
g.channel_steer.servo_out = nav_steer_cd;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
|
|
|
* Set the flight control servos based on the current calculated values
|
|
|
|
*****************************************/
|
|
|
|
static void set_servos(void)
|
|
|
|
{
|
2012-11-27 21:13:39 -04:00
|
|
|
int16_t last_throttle = g.channel_throttle.radio_out;
|
|
|
|
|
2013-03-14 21:04:33 -03:00
|
|
|
if ((control_mode == MANUAL || control_mode == LEARNING) &&
|
|
|
|
(g.skid_steer_out == g.skid_steer_in)) {
|
|
|
|
// do a direct pass through of radio values
|
|
|
|
g.channel_steer.radio_out = hal.rcin->read(CH_STEER);
|
|
|
|
g.channel_throttle.radio_out = hal.rcin->read(CH_THROTTLE);
|
2013-03-28 20:25:53 -03:00
|
|
|
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
|
|
|
|
// suppress throttle if in failsafe and manual
|
|
|
|
g.channel_throttle.radio_out = g.channel_throttle.radio_trim;
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
} else {
|
2013-02-07 18:21:22 -04:00
|
|
|
g.channel_steer.calc_pwm();
|
|
|
|
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
|
|
|
|
g.throttle_min.get(),
|
|
|
|
g.throttle_max.get());
|
2013-03-28 20:25:53 -03:00
|
|
|
|
|
|
|
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
|
|
|
|
// suppress throttle if in failsafe
|
|
|
|
g.channel_throttle.servo_out = 0;
|
|
|
|
}
|
|
|
|
|
2012-11-27 06:47:30 -04:00
|
|
|
// convert 0 to 100% into PWM
|
|
|
|
g.channel_throttle.calc_pwm();
|
2012-11-27 21:13:39 -04:00
|
|
|
|
|
|
|
// limit throttle movement speed
|
|
|
|
throttle_slew_limit(last_throttle);
|
2013-03-14 21:04:33 -03:00
|
|
|
|
|
|
|
if (g.skid_steer_out) {
|
|
|
|
// convert the two radio_out values to skid steering values
|
|
|
|
/*
|
|
|
|
mixing rule:
|
|
|
|
steering = motor1 - motor2
|
|
|
|
throttle = 0.5*(motor1 + motor2)
|
|
|
|
motor1 = throttle + 0.5*steering
|
|
|
|
motor2 = throttle - 0.5*steering
|
|
|
|
*/
|
|
|
|
float steering_scaled = g.channel_steer.norm_output();
|
|
|
|
float throttle_scaled = g.channel_throttle.norm_output();
|
|
|
|
float motor1 = throttle_scaled + 0.5*steering_scaled;
|
|
|
|
float motor2 = throttle_scaled - 0.5*steering_scaled;
|
|
|
|
g.channel_steer.servo_out = 4500*motor1;
|
|
|
|
g.channel_throttle.servo_out = 100*motor2;
|
|
|
|
g.channel_steer.calc_pwm();
|
|
|
|
g.channel_throttle.calc_pwm();
|
|
|
|
}
|
2012-11-27 06:47:30 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
|
|
|
// send values to the PWM timers for output
|
|
|
|
// ----------------------------------------
|
2013-02-07 18:21:22 -04:00
|
|
|
hal.rcout->write(CH_1, g.channel_steer.radio_out); // send to Servos
|
2012-12-18 07:44:12 -04:00
|
|
|
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-02-07 18:21:22 -04:00
|
|
|
// Route configurable aux. functions to their respective servos
|
|
|
|
g.rc_2.output_ch(CH_2);
|
|
|
|
g.rc_4.output_ch(CH_4);
|
2012-04-30 04:17:14 -03:00
|
|
|
g.rc_5.output_ch(CH_5);
|
|
|
|
g.rc_6.output_ch(CH_6);
|
|
|
|
g.rc_7.output_ch(CH_7);
|
|
|
|
g.rc_8.output_ch(CH_8);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2012-12-18 07:44:12 -04:00
|
|
|
static bool demoing_servos;
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2012-12-18 07:44:12 -04:00
|
|
|
static void demo_servos(uint8_t i) {
|
|
|
|
|
|
|
|
while(i > 0) {
|
|
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
|
|
|
|
demoing_servos = true;
|
2012-04-30 04:17:14 -03:00
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
2012-12-18 07:44:12 -04:00
|
|
|
hal.rcout->write(1, 1400);
|
|
|
|
mavlink_delay(400);
|
|
|
|
hal.rcout->write(1, 1600);
|
|
|
|
mavlink_delay(200);
|
|
|
|
hal.rcout->write(1, 1500);
|
2012-04-30 04:17:14 -03:00
|
|
|
#endif
|
2012-12-18 07:44:12 -04:00
|
|
|
demoing_servos = false;
|
|
|
|
mavlink_delay(400);
|
|
|
|
i--;
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|