diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:44:03 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:44:03 +0100 |
commit | dc100f20201974eeae8743e1184efe6be1e31e6f (patch) | |
tree | 438467d1ab1f8280f7e8c372581c0a7f019d2174 /apps/fixedwing_att_control/fixedwing_att_control_main.c | |
parent | 9ab20b11b6a528ef2ae4197e9cd412de52b1d024 (diff) | |
download | px4-firmware-dc100f20201974eeae8743e1184efe6be1e31e6f.tar.gz px4-firmware-dc100f20201974eeae8743e1184efe6be1e31e6f.tar.bz2 px4-firmware-dc100f20201974eeae8743e1184efe6be1e31e6f.zip |
Fixedwing controller code style
Diffstat (limited to 'apps/fixedwing_att_control/fixedwing_att_control_main.c')
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 333 |
1 files changed, 170 insertions, 163 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index a36f1ce7d..aa9db6d52 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -93,207 +93,211 @@ static int deamon_task; /**< Handle of deamon task / thread */ int fixedwing_att_control_thread_main(int argc, char *argv[]) { /* read arguments */ - bool verbose = false; + bool verbose = false; - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + verbose = true; } + } - /* welcome user */ - printf("[fixedwing att control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - - /* output structs */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - 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); - - /* subscribe */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - /* Check if there is a new position measurement or attitude setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool att_sp_updated; - orb_check(att_sp_sub, &att_sp_updated); - - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(att_sp_updated) - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - if(pos_updated) - { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if(att.R_valid) - { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - } - else - { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; + /* welcome user */ + printf("[fixedwing att control] started\n"); + + /* declare and safely initialize all structs */ + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + struct manual_control_setpoint_s manual_sp; + memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + + /* output structs */ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + actuators.control[i] = 0.0f; + } - printf("FW ATT CONTROL: Did not get a valid R\n"); - } + 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); + + /* subscribe */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* Setup of loop */ + float gyro[3] = {0.0f, 0.0f, 0.0f}; + float speed_body[3] = {0.0f, 0.0f, 0.0f}; + struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + + while (!thread_should_exit) { + /* wait for a sensor update, check for exit condition every 500 ms */ + poll(&fds, 1, 500); + + /* Check if there is a new position measurement or attitude setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool att_sp_updated; + orb_check(att_sp_sub, &att_sp_updated); + + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (att_sp_updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + + if (att.R_valid) { + speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; + speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; + speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; + + } else { + speed_body[0] = 0; + speed_body[1] = 0; + speed_body[2] = 0; + + printf("FW ATT CONTROL: Did not get a valid R\n"); } + } - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; - /* control */ + /* 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); + 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); + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; - /* set flaps to zero */ - actuators.control[4] = 0.0f; + /* 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) { + } 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; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (vstatus.rc_signal_lost) { - /* 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; + /* put plane into loiter */ + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; - // XXX disable yaw control, loiter + /* 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.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.thrust = 0.0f; } - att_sp.timestamp = hrt_absolute_time(); + att_sp.yaw_body = 0; - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + // XXX disable yaw control, loiter - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + } else { - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; + 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; + } - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - } else { - actuators.control[4] = 0.0f; - } + att_sp.timestamp = hrt_absolute_time(); - } 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; - } + /* 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; } - } - /* publish rates */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + } 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; - /* sanity check and publish actuator outputs */ - if (isfinite(actuators.control[0]) && - isfinite(actuators.control[1]) && - isfinite(actuators.control[2]) && - isfinite(actuators.control[3])) - { - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else { + actuators.control[4] = 0.0f; + } } } - printf("[fixedwing_att_control] exiting, stopping all motors.\n"); - thread_running = false; + /* publish rates */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + + /* sanity check and publish actuator outputs */ + if (isfinite(actuators.control[0]) && + isfinite(actuators.control[1]) && + isfinite(actuators.control[2]) && + isfinite(actuators.control[3])) { + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } + } + + printf("[fixedwing_att_control] exiting, stopping all motors.\n"); + thread_running = false; - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; + /* kill all outputs */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - close(att_sub); - close(actuator_pub); - close(rates_pub); + close(att_sub); + close(actuator_pub); + close(rates_pub); - fflush(stdout); - exit(0); + fflush(stdout); + exit(0); - return 0; + return 0; } @@ -304,6 +308,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n"); exit(1); } @@ -348,9 +353,11 @@ int fixedwing_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tfixedwing_att_control is running\n"); + } else { printf("\tfixedwing_att_control not started\n"); } + exit(0); } |