diff options
Diffstat (limited to 'src/modules/fixedwing_att_control')
3 files changed, 87 insertions, 86 deletions
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 2aeca3a98..68c176459 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 6c9c137bb..b96fc3e5a 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -186,87 +186,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } +#warning fix this +// if (vstatus.state_machine == SYSTEM_STATE_AUTO || +// vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* set flaps to zero */ +// actuators.control[4] = 0.0f; +// +// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { +// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +// if (vstatus.rc_signal_lost) { +// +// /* put plane into loiter */ +// att_sp.roll_body = 0.3f; +// att_sp.pitch_body = 0.0f; +// +// /* limit throttle to 60 % of last value if sane */ +// if (isfinite(manual_sp.throttle) && +// (manual_sp.throttle >= 0.0f) && +// (manual_sp.throttle <= 1.0f)) { +// att_sp.thrust = 0.6f * manual_sp.throttle; +// +// } else { +// att_sp.thrust = 0.0f; +// } +// +// att_sp.yaw_body = 0; +// +// // XXX disable yaw control, loiter +// +// } else { +// +// att_sp.roll_body = manual_sp.roll; +// att_sp.pitch_body = manual_sp.pitch; +// att_sp.yaw_body = 0; +// att_sp.thrust = manual_sp.throttle; +// } +// +// att_sp.timestamp = hrt_absolute_time(); +// +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* pass through flaps */ +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// +// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// /* directly pass through values */ +// actuators.control[0] = manual_sp.roll; +// /* positive pitch means negative actuator -> pull up */ +// actuators.control[1] = manual_sp.pitch; +// actuators.control[2] = manual_sp.yaw; +// actuators.control[3] = manual_sp.throttle; +// +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// } +// } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index cdab39edc..66854592a 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; |