From a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Sep 2012 18:44:01 +0200 Subject: Halfway-working fixed wing waypoint control, needs more effort --- apps/fixedwing_control/fixedwing_control.c | 1008 ++++++-------------- .../multirotor_att_control_main.c | 6 +- apps/multirotor_pos_control/position_control.c | 40 - apps/systemlib/Makefile | 3 +- apps/systemlib/geo/geo.c | 88 ++ apps/systemlib/geo/geo.h | 49 + 6 files changed, 409 insertions(+), 785 deletions(-) create mode 100644 apps/systemlib/geo/geo.c create mode 100644 apps/systemlib/geo/geo.h diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 67e5d1b28..1323f2566 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -44,16 +44,12 @@ #include #include #include -#include #include -#include +#include #include #include #include -#include -#include #include -#include #include #include #include @@ -62,6 +58,8 @@ #include #include #include +#include +#include static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,638 +80,75 @@ int fixedwing_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_control is running\n"); - } else { - printf("\tfixedwing_control not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -#define PID_DT 0.01f -#define PID_SCALER 0.1f -#define PID_DERIVMODE_CALC 0 -#define HIL_MODE 32 -#define AUTO -1000 -#define MANUAL 3000 -#define SERVO_MIN 1000 -#define SERVO_MAX 2000 - -pthread_t control_thread; -pthread_t nav_thread; -pthread_t servo_thread; - -/** - * Servo channels function enumerator used for - * the servo writing part - */ -enum SERVO_CHANNELS_FUNCTION { - - AIL_1 = 0, - AIL_2 = 1, - MOT = 2, - ACT_1 = 3, - ACT_2 = 4, - ACT_3 = 5, - ACT_4 = 6, - ACT_5 = 7 -}; - -/** - * The plane_data structure. - * - * The plane data structure is the local storage of all the flight information of the aircraft - */ -typedef struct { - double lat; - double lon; - float alt; - float vx; - float vy; - float vz; - float yaw; - float hdg; - float pitch; - float roll; - float yawspeed; - float pitchspeed; - float rollspeed; - float rollb; /* body frame angles */ - float pitchb; - float yawb; - float p; - float q; - float r; /* body angular rates */ - - /* PID parameters*/ - - float Kp_att; - float Ki_att; - float Kd_att; - float Kp_pos; - float Ki_pos; - float Kd_pos; - float intmax_att; - float intmax_pos; - - /* Next waypoint*/ - - double wp_x; - double wp_y; - float wp_z; - - /* Setpoints */ - - float airspeed; - float groundspeed; - float roll_setpoint; - float pitch_setpoint; - float throttle_setpoint; - - /* Navigation mode*/ - int mode; - -} plane_data_t; - -/** - * The control_outputs structure. - * - * The control outputs structure contains the control outputs - * of the aircraft - */ -typedef struct { - float roll_ailerons; - float pitch_elevator; - float yaw_rudder; - float throttle; - // set the aux values to 0 per default - float aux1; - float aux2; - float aux3; - float aux4; - uint8_t mode; // HIL_ENABLED: 32 - uint8_t nav_mode; -} control_outputs_t; - -/** - * Generic PID algorithm with PD scaling - */ -static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); - /* - * Output calculations - */ - -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z); -static void calc_bodyframe_angles(float roll, float pitch, float yaw); -static float calc_bearing(void); -static float calc_roll_ail(void); -static float calc_pitch_elev(void); -static float calc_yaw_rudder(float hdg); -static float calc_throttle(void); -static float calc_gnd_speed(void); -static void get_parameters(plane_data_t *plane_data); -static float calc_roll_setpoint(void); -static float calc_pitch_setpoint(void); -static float calc_throttle_setpoint(void); -static float calc_wp_distance(void); -static void set_plane_mode(void); - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -plane_data_t plane_data; -control_outputs_t control_outputs; -float scaler = 1; //M_PI; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/** - * Calculates the PID control output given an error. - * - * Incorporates PD scaling and low-pass filter for the derivative component. - * - * @param error the input error - * @param error_deriv the derivative of the input error - * @param dt time constant - * @param scaler PD scaler - * @param K_p P gain - * @param K_i I gain - * @param K_d D gain - * @param intmax Integration limit - * - * @return the PID control output - */ - -static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax) -{ - float Kp = K_p; - float Ki = K_i; - float Kd = K_d; - float delta_time = dt; - float lerror; - float imax = intmax; - float integrator; - float derivative; - float lderiv; - int fCut = 20; /* anything above 20 Hz is considered noise - low pass filter for the derivative */ - float output = 0; - - output += error * Kp; - - if ((fabsf(Kd) > 0) && (dt > 0)) { - - if (PID_DERIVMODE_CALC) { - derivative = (error - lerror) / delta_time; - - /* - * discrete low pass filter, cuts out the - * high frequency noise that can drive the controller crazy - */ - float RC = 1.0f / (2.0f * M_PI_F * fCut); - derivative = lderiv + - (delta_time / (RC + delta_time)) * (derivative - lderiv); - - /* update state */ - lerror = error; - lderiv = derivative; - - } else { - derivative = error_deriv; - } - - /* add in derivative component */ - output += Kd * derivative; - } - - //printf("PID derivative %i\n", (int)(1000*derivative)); - - /* scale the P and D components with the PD scaler */ - output *= scale; - - /* Compute integral component if time has elapsed */ - if ((fabsf(Ki) > 0) && (dt > 0)) { - integrator += (error * Ki) * scaler * delta_time; - - if (integrator < -imax) { - integrator = -imax; - - } else if (integrator > imax) { - integrator = imax; - } - - output += integrator; - } - - //printf("PID Integrator %i\n", (int)(1000*integrator)); - - return output; -} - -PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f); - -PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f); - -/** - * Load parameters from global storage. - * - * @param plane_data Fixed wing data structure - * - * Fetches the current parameters from the global parameter storage and writes them - * to the plane_data structure - */ -static void get_parameters(plane_data_t * pdata) -{ - static bool initialized = false; - static param_t att_p; - static param_t att_i; - static param_t att_d; - static param_t att_awu; - static param_t att_lim; - - static param_t pos_p; - static param_t pos_i; - static param_t pos_d; - static param_t pos_awu; - static param_t pos_lim; - - if (!initialized) { - att_p = param_find("FW_ATT_P"); - att_i = param_find("FW_ATT_I"); - att_d = param_find("FW_ATT_D"); - att_awu = param_find("FW_ATT_AWU"); - att_lim = param_find("FW_ATT_LIM"); - - pos_p = param_find("FW_POS_P"); - pos_i = param_find("FW_POS_I"); - pos_d = param_find("FW_POS_D"); - pos_awu = param_find("FW_POS_AWU"); - pos_lim = param_find("FW_POS_LIM"); - - initialized = true; - } - - param_get(att_p, &(pdata->Kp_att)); - param_get(att_i, &(pdata->Ki_att)); - param_get(att_d, &(pdata->Kd_att)); - param_get(pos_p, &(pdata->Kp_pos)); - param_get(pos_i, &(pdata->Ki_pos)); - param_get(pos_d, &(pdata->Kd_pos)); - param_get(att_awu, &(pdata->intmax_att)); - param_get(pos_awu, &(pdata->intmax_pos)); - pdata->airspeed = 10; - pdata->mode = 1; -} - -/** - * Calculates the body angular rates. - * - * Calculates the rates of the plane using inertia matrix and - * writes them to the plane_data structure - * - * @param roll - * @param pitch - * @param yaw - * @param rollspeed - * @param pitchspeed - * @param yawspeed - * - */ -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ - plane_data.p = rollspeed - sinf(pitch) * yawspeed; - plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed; - plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed; -} - -/** - * - * Calculates the attitude angles in the body reference frame. - * - * Writes them to the plane data structure - * - * @param roll - * @param pitch - * @param yaw - */ - -static void calc_bodyframe_angles(float roll, float pitch, float yaw) -{ - plane_data.rollb = cosf(yaw) * cosf(pitch) * roll + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * yaw; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw; - plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw; -} - -/** - * calc_rotation_matrix - * - * Calculates the rotation matrix - * - * @param roll - * @param pitch - * @param yaw - * @param x - * @param y - * @param z - * - */ - -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z) -{ - plane_data.rollb = cosf(yaw) * cosf(pitch) * x + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * z; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z; - plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z; -} - -/** - * calc_bearing - * - * Calculates the bearing error of the plane compared to the waypoints - * - * @return bearing Bearing error - * - */ -static float calc_bearing() -{ - float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)); - - if (bearing < 0.0f) { - bearing += 2*M_PI_F; - } - - return bearing; -} - -/** - * calc_roll_ail - * - * Calculates the roll ailerons control output - * - * @return Roll ailerons control output (-1,1) - */ - -static float calc_roll_ail() -{ - float ret = pid((plane_data.roll_setpoint - plane_data.roll), plane_data.rollspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_pitch_elev - * - * Calculates the pitch elevators control output + * Controller parameters, accessible via MAVLink * - * @return Pitch elevators control output (-1,1) */ -static float calc_pitch_elev() -{ - float ret = pid((plane_data.pitch_setpoint - plane_data.pitch), plane_data.pitchspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} +PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); + +struct fw_att_control_params { + float roll_pos_p; + float roll_pos_i; + float roll_pos_awu; + float roll_pos_lim; +}; -/** - * calc_yaw_rudder - * - * Calculates the yaw rudder control output (only if yaw rudder exists on the model) - * - * @return Yaw rudder control output (-1,1) - * - */ -static float calc_yaw_rudder(float hdg) -{ - float ret = pid((plane_data.yaw - abs(hdg)), plane_data.yawspeed, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); +struct fw_att_control_param_handles { + param_t roll_pos_p; + param_t roll_pos_i; + param_t roll_pos_awu; + param_t roll_pos_lim; +}; - if (ret < -1) - return -1; - if (ret > 1) - return 1; +// XXX outsource position control to a separate app some time - return ret; -} +PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); -/** - * calc_throttle - * - * Calculates the throttle control output - * - * @return Throttle control output (0,1) - */ - -static float calc_throttle() -{ - float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - - if (ret < 0.2f) - return 0.2f; - - if (ret > 1.0f) - return 1.0f; +struct fw_pos_control_params { + float heading_p; + float heading_lim; +}; - return ret; -} +struct fw_pos_control_param_handles { + param_t heading_p; + param_t heading_lim; +}; /** - * calc_gnd_speed - * - * Calculates the ground speed using the x and y components - * - * Input: none (operation on global data) - * - * Output: Ground speed of the plane + * Initialize all parameter handles and values * */ - -static float calc_gnd_speed() -{ - float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy); - return gnd_speed; -} +static int att_parameters_init(struct fw_att_control_param_handles *h); /** - * calc_wp_distance - * - * Calculates the distance to the next waypoint - * - * @return the distance to the next waypoint + * Update all parameters * */ - -static float calc_wp_distance() -{ - return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) + - (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x)); -} +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); /** - * calc_roll_setpoint - * - * Calculates the offset angle for the roll plane, - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing heading + * Initialize all parameter handles and values * */ - -static float calc_roll_setpoint() -{ - float setpoint = 0; - - setpoint = calc_bearing() - plane_data.yaw; - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - return setpoint; -} +static int pos_parameters_init(struct fw_pos_control_param_handles *h); /** - * calc_pitch_setpoint - * - * Calculates the offset angle for the pitch plane - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing altitude + * Update all parameters * */ +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); -static float calc_pitch_setpoint() -{ - float setpoint = 0.0f; - - // if (plane_data.mode == TAKEOFF) { - // setpoint = ((35/180.0f)*M_PI_F); - - // } else { - setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()); - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - // } - - return setpoint; -} /** + * The fixed wing control main loop. * - * Calculates the throttle setpoint for different flight modes - * - * @return throttle output setpoint - * - */ -static float calc_throttle_setpoint() -{ - float setpoint = 0; - - // // if TAKEOFF full throttle - // if (plane_data.mode == TAKEOFF) { - // setpoint = 60.0f; - // } - - // // if CRUISE - parameter value - // if (plane_data.mode == CRUISE) { - setpoint = plane_data.airspeed; - // } - - // // if LAND no throttle - // if (plane_data.mode == LAND) { - // setpoint = 0.0f; - // } - - return setpoint; -} - -/** + * This loop executes continously and calculates the control + * response. * * @param argc number of arguments * @param argv argument array @@ -723,27 +158,14 @@ static float calc_throttle_setpoint() */ int fixedwing_control_thread_main(int argc, char *argv[]) { - /* default values for arguments */ - char *fixedwing_uart_name = "/dev/ttyS1"; - char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n"; + // /* read arguments */ + // bool verbose = false; - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - fixedwing_uart_name = argv[i + 1]; - - } else { - printf(commandline_usage, argv[0]); - return 0; - } - } - 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 control] started\n"); @@ -760,10 +182,6 @@ int fixedwing_control_thread_main(int argc, char *argv[]) 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_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)); @@ -773,6 +191,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); /* subscribe to attitude, motor setpoints and system state */ + struct vehicle_global_position_s global_pos; + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_setpoint_s global_setpoint; + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); 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)); @@ -780,139 +202,241 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Mainloop setup */ unsigned int loopcounter = 0; - unsigned int failcounter = 0; - - /* Control constants */ - control_outputs.mode = HIL_MODE; - control_outputs.nav_mode = 0; - - /* Servo setup */ - - /* - * Main control, navigation and servo routine - */ - - while(1) { - /* - * DATA Handling - * Fetch current flight data - */ - - /* get position, attitude and rc inputs */ - // 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); - - /* 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) - */ - - /* position values*/ - plane_data.lat = global_pos.lat / 10000000.0; - plane_data.lon = global_pos.lon / 10000000.0; - plane_data.alt = global_pos.alt / 1000.0f; - plane_data.vx = global_pos.vx / 100.0f; - plane_data.vy = global_pos.vy / 100.0f; - plane_data.vz = global_pos.vz / 100.0f; - - /* attitude values*/ - plane_data.roll = att.roll; - plane_data.pitch = att.pitch; - plane_data.yaw = att.yaw; - plane_data.rollspeed = att.rollspeed; - plane_data.pitchspeed = att.pitchspeed; - plane_data.yawspeed = att.yawspeed; - - plane_data.wp_x = global_setpoint.lat / (double)1e7; - plane_data.wp_y = global_setpoint.lon / (double)1e7; - plane_data.wp_z = global_setpoint.altitude; - - /* parameter values */ - if (loopcounter % 20 == 0) { - get_parameters(&plane_data); - } - /* Attitude control part */ + uint64_t last_run = 0; + uint64_t last_run_pos = 0; - if (verbose && loopcounter % 20 == 0) { - /******************************** DEBUG OUTPUT ************************************************************/ + bool global_sp_updated_set_once = false; - 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); + struct fw_att_control_params p; + struct fw_att_control_param_handles h; - printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); - printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); - printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*calc_throttle_setpoint())); + struct fw_pos_control_params ppos; + struct fw_pos_control_param_handles hpos; - // printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); + /* initialize the pid controllers */ + att_parameters_init(&h); + att_parameters_update(&h, &p); - // printf("Current Altitude: %i\n\n", (int)plane_data.alt); + pos_parameters_init(&hpos); + pos_parameters_update(&hpos, &ppos); - printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", - (int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F), - (int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F); + PID_t roll_pos_controller; + pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, + PID_MODE_DERIVATIV_SET, 155); - // 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)); + PID_t heading_controller; + pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, + PID_MODE_DERIVATIV_SET, 155); - 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); + while(!thread_should_exit) { - /************************************************************************************************************/ - } + struct pollfd fds[1] = { + { .fd = att_sub, .events = POLLIN }, + }; + int ret = poll(fds, 1, 1000); - /* Calculate the P,Q,R body rates of the aircraft */ - //calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw, - // plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed); + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); + } else { - /* Calculate the body frame angles of the aircraft */ - //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); + // FIXME SUBSCRIBE + if (loopcounter % 20 == 0) { + pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); + } - // if (manual.override_mode_switch < XXX) { + /* if position updated, run position control loop */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + if (global_sp_updated) { + global_sp_updated_set_once = true; + } + /* checking has to happen before the read, as the read clears the changed flag */ + + /* 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); + + if (pos_updated) { + + /* get position */ + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + + if (global_sp_updated_set_once) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + + + /* calculate delta T */ + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* calculate bearing error */ + float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, + global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + + /* shift error to prevent wrapping issues */ + float bearing_error = att.yaw - target_bearing; + + if (bearing_error < M_PI_F) { + bearing_error += 2.0f * M_PI_F; + } + + if (bearing_error > M_PI_F) { + bearing_error -= 2.0f * M_PI_F; + } + + /* calculate roll setpoint, do this artificially around zero */ + att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, + 0.0f, att.yawspeed, deltaT); + + /* limit roll angle output */ + if (att_sp.roll_body > ppos.heading_lim) { + att_sp.roll_body = ppos.heading_lim; + heading_controller.saturated = 1; + } + + if (att_sp.roll_body < -ppos.heading_lim) { + att_sp.roll_body = -ppos.heading_lim; + heading_controller.saturated = 1; + } + + att_sp.roll_body = att_sp.roll_body; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + + } else { + /* no setpoint, maintain level flight */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + } + + att_sp.thrust = 0.7f; + } + /* calculate delta T */ + const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; + last_run_pos = hrt_absolute_time(); - /* Navigation part */ + actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, + att.roll, att.rollspeed, deltaTpos); + actuators.control[1] = 0; + actuators.control[2] = 0; + actuators.control[3] = att_sp.thrust; - // Get Waypoint + /* publish attitude setpoint (for MAVLink) */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // XXX FIXME + /* publish actuator setpoints (for mixer) */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - //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(); + loopcounter++; - /* 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(); + } + } - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + printf("[fixedwing_control] exiting.\n"); + thread_running = false; - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + return 0; +} - loopcounter++; +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int fixedwing_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); - /* 20Hz loop*/ - usleep(50000); + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("fixedwing_control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + thread_running = true; + exit(0); } - return 0; + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tfixedwing_control is running\n"); + } else { + printf("\tfixedwing_control not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int att_parameters_init(struct fw_att_control_param_handles *h) +{ + /* PID parameters */ + h->roll_pos_p = param_find("FW_ROLL_POS_P"); + h->roll_pos_i = param_find("FW_ROLL_POS_I"); + h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); + h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); + + return OK; +} + +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) +{ + param_get(h->roll_pos_p, &(p->roll_pos_p)); + + return OK; +} + +static int pos_parameters_init(struct fw_pos_control_param_handles *h) +{ + /* PID parameters */ + h->heading_p = param_find("FW_HEADING_P"); + h->heading_lim = param_find("FW_HEADING_LIM"); + + return OK; +} + +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) +{ + param_get(h->heading_p, &(p->heading_p)); + param_get(h->heading_lim, &(p->heading_lim)); + + return OK; } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 6cf811158..5bc0d5fa3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,8 @@ * @file multirotor_att_control_main.c * * Implementation of multirotor attitude control main loop. + * + * @author Lorenz Meier */ #include diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c index fd731a33d..b98f5bede 100644 --- a/apps/multirotor_pos_control/position_control.c +++ b/apps/multirotor_pos_control/position_control.c @@ -51,46 +51,6 @@ #include "multirotor_position_control.h" -float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; -} - -float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - // XXX wrapping check is incomplete - if (theta < 0.0f) { - theta = theta + 2.0f * M_PI_F; - } - - return theta; -} - void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index ec6bb3fb3..b49a3c54a 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -48,7 +48,8 @@ CSRCS = err.c \ # ifeq ($(TARGET),px4fmu) CSRCS += systemlib.c \ - pid/pid.c + pid/pid.c \ + geo/geo.c endif include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c new file mode 100644 index 000000000..42d8d9c15 --- /dev/null +++ b/apps/systemlib/geo/geo.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#include +#include + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + // XXX wrapping check is incomplete + if (theta < 0.0f) { + theta = theta + 2.0f * M_PI_F; + } + + return theta; +} \ No newline at end of file diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h new file mode 100644 index 000000000..0e86b3599 --- /dev/null +++ b/apps/systemlib/geo/geo.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -- cgit v1.2.3