diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index bdf7976736..beabc1142d 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -60,6 +60,7 @@ #include #include #include +#include static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -158,14 +159,14 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s */ int fixedwing_control_thread_main(int argc, char *argv[]) { - // /* read arguments */ - // bool verbose = false; + /* read arguments */ + 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 control] started\n"); @@ -229,6 +230,11 @@ int fixedwing_control_thread_main(int argc, char *argv[]) pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, PID_MODE_DERIVATIV_SET); + // XXX remove in production + /* advertise debug value */ + struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + while(!thread_should_exit) { struct pollfd fds[1] = { @@ -268,7 +274,11 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* 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); + //orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + // XXX update to switch between external attitude reference and the + // attitude calculated here + + char name[10]; if (pos_updated) { @@ -288,7 +298,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); /* shift error to prevent wrapping issues */ - float bearing_error = att.yaw - target_bearing; + float bearing_error = target_bearing - att.yaw; + + if (loopcounter % 2 == 0) { + sprintf(name, "hdng err1"); + memcpy(dbg.key, name, sizeof(name)); + dbg.value = bearing_error; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + } if (bearing_error < M_PI_F) { bearing_error += 2.0f * M_PI_F; @@ -298,6 +315,13 @@ int fixedwing_control_thread_main(int argc, char *argv[]) bearing_error -= 2.0f * M_PI_F; } + if (loopcounter % 2 != 0) { + sprintf(name, "hdng err2"); + memcpy(dbg.key, name, sizeof(name)); + dbg.value = bearing_error; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + } + /* calculate roll setpoint, do this artificially around zero */ att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, 0.0f, att.yawspeed, deltaT); @@ -313,7 +337,6 @@ int fixedwing_control_thread_main(int argc, char *argv[]) heading_controller.saturated = 1; } - att_sp.roll_body = att_sp.roll_body; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; @@ -331,7 +354,13 @@ int fixedwing_control_thread_main(int argc, char *argv[]) const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; last_run_pos = hrt_absolute_time(); - actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, + if (verbose && (loopcounter % 20 == 0)) { + printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); + } + + // actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, + // att.roll, att.rollspeed, deltaTpos); + 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; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index a1458ca03b..ea1c511f91 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1416,15 +1416,15 @@ void handleMessage(mavlink_message_t *msg) memset(&rc_hil, 0, sizeof(rc_hil)); static orb_advert_t rc_pub = 0; - rc_hil.chan[0].raw = 1510 + man.x / 2; - rc_hil.chan[1].raw = 1520 + man.y / 2; - rc_hil.chan[2].raw = 1590 + man.r / 2; - rc_hil.chan[3].raw = 1420 + man.z / 2; + rc_hil.chan[0].raw = 1500 + man.x / 2; + rc_hil.chan[1].raw = 1500 + man.y / 2; + rc_hil.chan[2].raw = 1500 + man.r / 2; + rc_hil.chan[3].raw = 1500 + man.z / 2; - rc_hil.chan[0].scaled = man.x; - rc_hil.chan[1].scaled = man.y; - rc_hil.chan[2].scaled = man.r; - rc_hil.chan[3].scaled = man.z; + rc_hil.chan[0].scaled = man.x / 1000.0f; + rc_hil.chan[1].scaled = man.y / 1000.0f; + rc_hil.chan[2].scaled = man.r / 1000.0f; + rc_hil.chan[3].scaled = man.z / 1000.0f; struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; @@ -1432,7 +1432,7 @@ void handleMessage(mavlink_message_t *msg) mc.roll = man.x / 1000.0f; mc.pitch = man.y / 1000.0f; mc.yaw = man.r / 1000.0f; - mc.roll = man.z / 1000.0f; + mc.throttle = man.z / 1000.0f; /* fake RC channels with manual control input from simulator */ diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index a9aab04320..48d338c992 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -66,7 +66,6 @@ #include #include - class FMUServo : public device::CDev { public: @@ -75,7 +74,7 @@ public: MODE_4PWM, MODE_NONE }; - FMUServo(Mode mode); + FMUServo(Mode mode, int update_rate); ~FMUServo(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); @@ -86,6 +85,7 @@ private: static const unsigned _max_actuators = 4; Mode _mode; + int _update_rate; int _task; int _t_actuators; int _t_armed; @@ -115,9 +115,10 @@ FMUServo *g_servo; } // namespace -FMUServo::FMUServo(Mode mode) : +FMUServo::FMUServo(Mode mode, int update_rate) : CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH), _mode(mode), + _update_rate(update_rate), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -199,6 +200,8 @@ FMUServo::task_main() /* multi-port as 4 PWM outs */ /* XXX magic numbers */ up_pwm_servo_init(0xf); + /* set the update rate for 4 PWM mode only for now */ + up_pwm_servo_set_rate(_update_rate); break; case MODE_NONE: @@ -208,7 +211,9 @@ FMUServo::task_main() /* subscribe to objects that we are interested in watching */ _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(_t_actuators, 20); /* 50Hz update rate */ + /* convert the update rate in hz to milliseconds, rounding down if necessary */ + int update_rate_in_ms = int(1000 / _update_rate); + orb_set_interval(_t_actuators, update_rate_in_ms); _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ @@ -444,7 +449,7 @@ enum PortMode { PortMode g_port_mode; int -fmu_new_mode(PortMode new_mode) +fmu_new_mode(PortMode new_mode, int update_rate) { int fd; int ret = OK; @@ -511,7 +516,7 @@ fmu_new_mode(PortMode new_mode) /* create new PWM driver if required */ if (servo_mode != FMUServo::MODE_NONE) { - g_servo = new FMUServo(servo_mode); + g_servo = new FMUServo(servo_mode, update_rate); if (g_servo == nullptr) { ret = -ENOMEM; @@ -585,6 +590,7 @@ int fmu_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; + int pwm_update_rate_in_hz = 50; if (!strcmp(argv[1], "test")) test(); @@ -597,24 +603,40 @@ fmu_main(int argc, char *argv[]) * * XXX use getopt? */ - if (!strcmp(argv[1], "mode_gpio")) { - new_mode = PORT_FULL_GPIO; + for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ + if (!strcmp(argv[i], "mode_gpio")) { + new_mode = PORT_FULL_GPIO; - } else if (!strcmp(argv[1], "mode_serial")) { - new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(argv[i], "mode_serial")) { + new_mode = PORT_FULL_SERIAL; - } else if (!strcmp(argv[1], "mode_pwm")) { - new_mode = PORT_FULL_PWM; + } else if (!strcmp(argv[i], "mode_pwm")) { + new_mode = PORT_FULL_PWM; - } else if (!strcmp(argv[1], "mode_gpio_serial")) { - new_mode = PORT_GPIO_AND_SERIAL; + } else if (!strcmp(argv[i], "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; - } else if (!strcmp(argv[1], "mode_pwm_serial")) { - new_mode = PORT_PWM_AND_SERIAL; + } else if (!strcmp(argv[i], "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; - } else if (!strcmp(argv[1], "mode_pwm_gpio")) { - new_mode = PORT_PWM_AND_GPIO; - } + } else if (!strcmp(argv[i], "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; + } + + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { + if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { + if (argc > i + 1) { + pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + } else { + fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + } + } + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNSET) { @@ -624,12 +646,12 @@ fmu_main(int argc, char *argv[]) return OK; /* switch modes */ - return fmu_new_mode(new_mode); + return fmu_new_mode(new_mode, pwm_update_rate_in_hz); } /* test, etc. here */ fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-r pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); return -EINVAL; } diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 42d8d9c150..ce46d01cc8 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -79,9 +79,12 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* 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; + if (theta < M_PI_F) { + theta += 2.0f * M_PI_F; + } + + if (theta > M_PI_F) { + theta -= 2.0f * M_PI_F; } return theta; diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 61dd5757fb..cff5e6bbe2 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -179,7 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous = error; } - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { pid->last_output = output; diff --git a/nuttx/configs/px4fmu/src/up_pwm_servo.c b/nuttx/configs/px4fmu/src/up_pwm_servo.c index adb9b50308..d278da55c3 100644 --- a/nuttx/configs/px4fmu/src/up_pwm_servo.c +++ b/nuttx/configs/px4fmu/src/up_pwm_servo.c @@ -179,7 +179,7 @@ static void pwm_timer_set_rate(unsigned timer, unsigned rate) { /* configure the timer to update at the desired rate */ - rARR(timer) = 1000000 / pwm_update_rate; + rARR(timer) = 1000000 / rate; /* generate an update event; reloads the counter and all registers */ rEGR(timer) = GTIM_EGR_UG;