diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:16 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:42 +0200 |
commit | a1b99a3f03a5908ea7e263658343451440326aea (patch) | |
tree | 45b3d287c854c99d0655934dbbd4ca65238703b8 /apps/fixedwing_control/fixedwing_control.c | |
parent | a69c55f671b1a2116c0e6c497906844a81ce6c74 (diff) | |
download | px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.gz px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.bz2 px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.zip |
Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control
Diffstat (limited to 'apps/fixedwing_control/fixedwing_control.c')
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 239 |
1 files changed, 67 insertions, 172 deletions
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 1d13b1c6a..73a72185d 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -52,14 +52,15 @@ #include <arch/board/board.h> #include <drivers/drv_pwm_output.h> #include <nuttx/spi.h> -#include "../mix_and_link/mix_and_link.h" //TODO: add to Makefile #include <uORB/uORB.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/fixedwing_control.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> #include <systemlib/param/param.h> __EXPORT int fixedwing_control_main(int argc, char *argv[]); @@ -710,19 +711,38 @@ int fixedwing_control_main(int argc, char *argv[]) /* welcome user */ printf("[fixedwing control] started\n"); - /* Set up to publish fixed wing control messages */ - struct fixedwing_control_s control; - orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + /* output structs */ + struct actuator_controls_s actuators; + struct actuator_armed_s armed; + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + /* 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); + armed.armed = false; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); /* Subscribe to global position, attitude and rc */ struct vehicle_global_position_s global_pos; int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_attitude_s att; - int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - struct rc_channels_s rc; - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); struct vehicle_global_position_setpoint_s global_setpoint; int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct vehicle_attitude_s att; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* Mainloop setup */ unsigned int loopcounter = 0; @@ -734,44 +754,6 @@ int fixedwing_control_main(int argc, char *argv[]) /* Servo setup */ - int fd; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; - - fd = open("/dev/pwm_servo", O_RDWR); - - if (fd < 0) { - printf("[fixedwing control] Failed opening /dev/pwm_servo\n"); - } - - ioctl(fd, PWM_SERVO_ARM, 0); - - int16_t buffer_rc[3]; - int16_t buffer_servo[3]; - mixer_data_t mixer_buffer; - mixer_buffer.input = buffer_rc; - mixer_buffer.output = buffer_servo; - - mixer_conf_t mixers[3]; - - mixers[0].source = PITCH; - mixers[0].nr_actuators = 2; - mixers[0].dest[0] = AIL_1; - mixers[0].dest[1] = AIL_2; - mixers[0].dual_rate[0] = 1; - mixers[0].dual_rate[1] = 1; - - mixers[1].source = ROLL; - mixers[1].nr_actuators = 2; - mixers[1].dest[0] = AIL_1; - mixers[1].dest[1] = AIL_2; - mixers[1].dual_rate[0] = 1; - mixers[1].dual_rate[1] = -1; - - mixers[2].source = THROTTLE; - mixers[2].nr_actuators = 1; - mixers[2].dest[0] = MOT; - mixers[2].dual_rate[0] = 1; - /* * Main control, navigation and servo routine */ @@ -786,8 +768,15 @@ int fixedwing_control_main(int argc, char *argv[]) // XXX add error checking orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att); - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); /* scaling factors are defined by the data from the APM Planner * TODO: ifdef for other parameters (HIL/Real world switch) @@ -810,20 +799,19 @@ int fixedwing_control_main(int argc, char *argv[]) plane_data.yawspeed = att.yawspeed; /* parameter values */ - get_parameters(&plane_data); + if (loopcounter % 20 == 0) { + get_parameters(&plane_data); + } /* Attitude control part */ if (verbose && loopcounter % 20 == 0) { /******************************** DEBUG OUTPUT ************************************************************/ - printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, + printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, - (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z, - (int)global_data_parameter_storage->pm.param_values[PARAM_WPLON], - (int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT], - (int)global_data_parameter_storage->pm.param_values[PARAM_WPALT]); + (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); @@ -840,20 +828,13 @@ int fixedwing_control_main(int argc, char *argv[]) // printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", // (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); - printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n", - (int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator), - (int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle)); + printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n", + att_sp.roll_body, att_sp.pitch_body, + att_sp.yaw_body, att_sp.thrust); /************************************************************************************************************/ } - /* - * Computation section - * - * The function calls to compute the required control values happen - * in this section. - */ - /* Set plane mode */ set_plane_mode(); @@ -864,123 +845,37 @@ int fixedwing_control_main(int argc, char *argv[]) /* Calculate the body frame angles of the aircraft */ //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); - /* Calculate the output values */ - control_outputs.roll_ailerons = calc_roll_ail(); - control_outputs.pitch_elevator = calc_pitch_elev(); - //control_outputs.yaw_rudder = calc_yaw_rudder(); - control_outputs.throttle = calc_throttle(); - - if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode + // if (manual.override_mode_switch < XXX) { - /* - * TAKEOFF hack for HIL - */ - if (plane_data.mode == TAKEOFF) { - control.attitude_control_output[ROLL] = 0; - control.attitude_control_output[PITCH] = 5000; - control.attitude_control_output[THROTTLE] = 10000; - //global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); - } - - if (plane_data.mode == CRUISE) { - control.attitude_control_output[ROLL] = control_outputs.roll_ailerons; - control.attitude_control_output[PITCH] = control_outputs.pitch_elevator; - control.attitude_control_output[THROTTLE] = control_outputs.throttle; - //control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); - } - - control.counter++; - control.timestamp = hrt_absolute_time(); - } /* Navigation part */ - // Get GPS Waypoint - - // if(global_data_wait(&global_data_position_setpoint->access_conf) == 0) - // { - // plane_data.wp_x = global_data_position_setpoint->x; - // plane_data.wp_y = global_data_position_setpoint->y; - // plane_data.wp_z = global_data_position_setpoint->z; - // } - // global_data_unlock(&global_data_position_setpoint->access_conf); - - if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode - // AUTO/HYBRID switch - - if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) { - plane_data.roll_setpoint = calc_roll_setpoint(); - plane_data.pitch_setpoint = calc_pitch_setpoint(); - plane_data.throttle_setpoint = calc_throttle_setpoint(); - - } else { - plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200; - plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200; - plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200; - } - - //control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg); - - } else { - control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000; - control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000; - control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000; - // since we don't have a yaw rudder - //control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale; - - control.counter++; - control.timestamp = hrt_absolute_time(); - } - - /* publish the control data */ + // Get Waypoint - orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control); + // XXX FIXME - /* Servo part */ + //else if (manual.override_mode_switch > MANUAL) { // AUTO mode + + /* calculate setpoint based on current position and waypoint */ + att_sp.roll_body = calc_roll_setpoint(); + att_sp.pitch_body = calc_pitch_setpoint(); + att_sp.thrust = calc_throttle_setpoint(); - buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]); - buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]); - buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]); + /* calculate the control outputs based on roll / pitch / yaw setpoints */ + actuators.control[0] = calc_roll_ail(); + actuators.control[1] = calc_pitch_elev(); + actuators.control[2] = calc_yaw_rudder(att.yaw); + actuators.control[3] = calc_throttle(); - //mix_and_link(mixers, 3, 2, &mixer_buffer); + /* publish attitude setpoint (for MAVLink) */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // Scaling and saturation of servo outputs happens here + /* publish actuator setpoints (for mixer) */ - data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM]; - - if (data[AIL_1] > SERVO_MAX) - data[AIL_1] = SERVO_MAX; - - if (data[AIL_1] < SERVO_MIN) - data[AIL_1] = SERVO_MIN; - - data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM]; - - if (data[AIL_2] > SERVO_MAX) - data[AIL_2] = SERVO_MAX; - - if (data[AIL_2] < SERVO_MIN) - data[AIL_2] = SERVO_MIN; - - data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM]; - - if (data[MOT] > SERVO_MAX) - data[MOT] = SERVO_MAX; - - if (data[MOT] < SERVO_MIN) - data[MOT] = SERVO_MIN; - - int result = write(fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - if (failcounter < 10 || failcounter % 20 == 0) { - printf("[fixedwing_control] failed writing servo outputs\n"); - } - failcounter++; - } + /* arming state depends on commander arming state */ + armed.armed = state.flag_system_armed; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); loopcounter++; |