diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 97049318dd..3bea67a315 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -77,6 +77,7 @@ set(config_module_list examples/mc_pos_control_multiplatform examples/ekf_att_pos_estimator examples/attitude_estimator_ekf + examples/fixedwing_control # # Testing diff --git a/src/examples/fixedwing_control/CMakeLists.txt b/src/examples/fixedwing_control/CMakeLists.txt index bc0e68fbfd..0f1dcd70e6 100644 --- a/src/examples/fixedwing_control/CMakeLists.txt +++ b/src/examples/fixedwing_control/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( STACK_MAIN 1200 STACK_MAX 1300 SRCS - main.c + main.cpp params.c DEPENDS platforms__common diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.cpp similarity index 92% rename from src/examples/fixedwing_control/main.c rename to src/examples/fixedwing_control/main.cpp index f8a9de39ce..83b465481c 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.cpp @@ -71,11 +71,25 @@ #include #include +#include + /* process-specific header files */ #include "params.h" /* Prototypes */ +/** + * Initialize all parameter handles and values + * + */ +extern "C" int parameters_init(struct param_handles *h); + +/** + * Update all parameters + * + */ +extern "C" int parameters_update(const struct param_handles *h, struct params *p); + /** * Daemon management function. * @@ -85,7 +99,7 @@ * the command line to one particular process or the need for bg/fg * ^Z support by the shell. */ -__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]); +extern "C" __EXPORT int ex_fixedwing_control_main(int argc, char *argv[]); /** * Mainloop of daemon. @@ -162,13 +176,20 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st /* * Calculate roll error and apply P gain */ - float roll_err = att->roll - att_sp->roll_body; + + matrix::Quaternion qa(&att->q[0]); + matrix::Euler att_euler(qa); + + matrix::Quaternion qd(&att_sp->q_d[0]); + matrix::Euler att_sp_euler(qd); + + float roll_err = att_euler(0) - att_sp_euler(0); actuators->control[0] = roll_err * p.roll_p; /* * Calculate pitch error and apply P gain */ - float pitch_err = att->pitch - att_sp->pitch_body; + float pitch_err = att_euler(1) - att_sp_euler(1); actuators->control[1] = pitch_err * p.pitch_p; } @@ -182,18 +203,30 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); + matrix::Quaternion qa(&att->q[0]); + matrix::Euler att_euler(qa); + /* calculate heading error */ - float yaw_err = att->yaw - bearing; + float yaw_err = att_euler(2) - bearing; /* apply control gain */ - att_sp->roll_body = yaw_err * p.hdng_p; + float roll_body = yaw_err * p.hdng_p; /* limit output, this commonly is a tuning parameter, too */ - if (att_sp->roll_body < -0.6f) { - att_sp->roll_body = -0.6f; + if (roll_body < -0.6f) { + roll_body = -0.6f; } else if (att_sp->roll_body > 0.6f) { - att_sp->roll_body = 0.6f; + roll_body = 0.6f; } + + matrix::Euler att_spe(roll_body, 0, bearing); + + matrix::Quaternion qd(att_spe); + + att_sp->q_d[0] = qd(0); + att_sp->q_d[1] = qd(1); + att_sp->q_d[2] = qd(2); + att_sp->q_d[3] = qd(3); } /* Main Thread */ @@ -262,7 +295,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* publish actuator controls with zero values */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } @@ -380,7 +413,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) thread_running = false; /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } @@ -456,6 +489,3 @@ int ex_fixedwing_control_main(int argc, char *argv[]) usage("unrecognized command"); exit(1); } - - - diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index fbbd30201e..88bd5cfcf8 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -57,6 +57,14 @@ PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f); */ PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f); +int parameters_init(struct param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct param_handles *h, struct params *p); + int parameters_init(struct param_handles *h) { /* PID parameters */ @@ -64,7 +72,7 @@ int parameters_init(struct param_handles *h) h->roll_p = param_find("EXFW_ROLL_P"); h->pitch_p = param_find("EXFW_PITCH_P"); - return OK; + return 0; } int parameters_update(const struct param_handles *h, struct params *p) @@ -73,5 +81,5 @@ int parameters_update(const struct param_handles *h, struct params *p) param_get(h->roll_p, &(p->roll_p)); param_get(h->pitch_p, &(p->pitch_p)); - return OK; + return 0; } diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 49ffff4467..8f8186b5c8 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -51,15 +51,3 @@ struct param_handles { param_t roll_p; param_t pitch_p; }; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct param_handles *h, struct params *p);