diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-26 21:02:36 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-26 21:02:36 +0100 |
commit | 80b84819d2efe8596c105db2aa698dd976c338c3 (patch) | |
tree | 130ca8a769594d6450c1128c440039ff0a935056 /apps/fixedwing_att_control | |
parent | eca12343fd7c315dea56845c2693041632a207ee (diff) | |
parent | 4366d9e31904a9b96992e1bb41388bd69ba92407 (diff) | |
download | px4-firmware-80b84819d2efe8596c105db2aa698dd976c338c3.tar.gz px4-firmware-80b84819d2efe8596c105db2aa698dd976c338c3.tar.bz2 px4-firmware-80b84819d2efe8596c105db2aa698dd976c338c3.zip |
Merged fixed wing branches
Diffstat (limited to 'apps/fixedwing_att_control')
3 files changed, 9 insertions, 11 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index a3c7df69d..d27f50b43 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -146,6 +146,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); + /* Pitch (P) */ /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ @@ -156,7 +157,9 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* 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)) - / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[1] * sinf(att->pitch)); + / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); + +// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index e1776f74a..8773db9b6 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -142,8 +142,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&manual_sp, 0, sizeof(manual_sp)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); - struct debug_key_value_s debug_output; - memset(&debug_output, 0, sizeof(debug_output)); /* output structs */ struct actuator_controls_s actuators; @@ -156,8 +154,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); - strncpy(debug_output.key, "yaw_rate", sizeof(debug_output.key - 1)); /* subscribe */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -259,7 +255,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* pass through throttle */ actuators.control[3] = att_sp.thrust; - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { /* directly pass through values */ actuators.control[0] = manual_sp.roll; @@ -269,10 +264,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[3] = manual_sp.throttle; } - /* publish output */ + /* publish rates */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + + /* publish actuator outputs */ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - debug_output.value = rates_sp.yaw; - orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); } printf("[fixedwing_att_control] exiting, stopping all motors.\n"); diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index 0bb54624f..e8277f3de 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -169,9 +169,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } - /* Roll Rate (PI) */ + /* roll rate (PI) */ actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - /* pitch rate (PI) */ actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* yaw rate (PI) */ |