diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 4152494e0e9b..69d791da52c7 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ] then echo "[init] reading /fs/microsd/etc/rc" sh /fs/microsd/etc/rc -else - echo "[init] script /fs/microsd/etc/rc not present" +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt fi # diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6533390bc057..1c3b9976b3ba 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -35,7 +35,7 @@ /* * @file attitude_estimator_ekf_main.c - * + * * Extended Kalman Filter for Attitude Estimation. */ @@ -79,18 +79,18 @@ static float dt = 0.005f; static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ static float x_aposteriori_k[12]; /**< states */ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, -}; /**< init: diagonal matrix with big values */ + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ static float x_aposteriori[12]; static float P_aposteriori[144]; @@ -99,9 +99,9 @@ static float P_aposteriori[144]; static float euler[3] = {0.0f, 0.0f, 0.0f}; static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f -}; /**< init: identity matrix */ + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -123,6 +123,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); exit(1); } @@ -131,7 +132,7 @@ usage(const char *reason) * The attitude_estimator_ekf 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(). */ @@ -150,11 +151,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12000, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12000, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -166,9 +167,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tattitude_estimator_ekf app is running\n"); + } else { printf("\tattitude_estimator_ekf app not started\n"); } + exit(0); } @@ -235,7 +238,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* advertise debug value */ // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; - + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs @@ -263,8 +266,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) while (!thread_should_exit) { struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } + { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } }; int ret = poll(fds, 2, 1000); @@ -273,10 +276,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_status), sub_state, &state); + if (!state.flag_hil_enabled) { - fprintf(stderr, + fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } + } else { /* only update parameters if they changed */ @@ -308,6 +313,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; } + } else { perf_begin(ekf_loop_perf); @@ -336,6 +342,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.timestamp; } + z_k[3] = raw.accelerometer_m_s2[0]; z_k[4] = raw.accelerometer_m_s2[1]; z_k[5] = raw.accelerometer_m_s2[2]; @@ -347,6 +354,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.timestamp; } + z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; @@ -368,8 +376,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) - { + if (!const_initialized && dt < 0.05f && dt > 0.005f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -395,13 +402,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } uint64_t timing_start = hrt_absolute_time(); - + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; @@ -413,9 +422,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* send out */ att.timestamp = raw.timestamp; - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2]; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -431,6 +442,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + } else { warnx("NaN in roll/pitch/yaw estimate!"); } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4fc2e0452a19..91fec9ccbe07 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -35,7 +35,7 @@ /* * @file attitude_estimator_ekf_params.c - * + * * Parameters for EKF filter */ @@ -65,13 +65,17 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); /* magnetometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { @@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r7 = param_find("EKF_ATT_R7"); h->r8 = param_find("EKF_ATT_R8"); + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + return OK; } @@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r7, &(p->r[7])); param_get(h->r8, &(p->r[8])); + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + return OK; } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 6a63f97674ee..b315d5fe8378 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -35,7 +35,7 @@ /* * @file attitude_estimator_ekf_params.h - * + * * Parameters for EKF filter */ @@ -44,11 +44,15 @@ struct attitude_estimator_ekf_params { float r[9]; float q[12]; + float roll_off; + float pitch_off; + float yaw_off; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; + param_t roll_off, pitch_off, yaw_off; }; /** diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c index dbf6cacafda7..a26938637841 100644 --- a/apps/commander/calibration_routines.c +++ b/apps/commander/calibration_routines.c @@ -45,172 +45,175 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) { - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A*A; - float B2 = B*B; - float C2 = C*C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * ( QS - Rsq + QB + F0 ); - float aA,aB,aC,nA,nB,nC,dA,dB,dC; - - //Iterate N times, ignore stop condition. - int n = 0; - while( n < max_iterations ){ - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2); - aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2); - aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA); - nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB); - nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A*A; - B2 = B*B; - C2 = C*C; - QS = A2 + B2 + C2; - QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * ( QS - Rsq + QB + F0 ); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) +{ + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A * A; + float B2 = B * B; + float C2 = C * C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * (QS - Rsq + QB + F0); + float aA, aB, aC, nA, nB, nC, dA, dB, dC; + + //Iterate N times, ignore stop condition. + int n = 0; + + while (n < max_iterations) { + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); + aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); + aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); + nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); + nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + + if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A * A; + B2 = B * B; + C2 = C * C; + QS = A2 + B2 + C2; + QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * (QS - Rsq + QB + F0); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; } diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h index 06382310959f..e3e7fbafdd72 100644 --- a/apps/commander/calibration_routines.h +++ b/apps/commander/calibration_routines.h @@ -58,4 +58,4 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 986266843ad8..c05948402499 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -73,10 +72,12 @@ #include #include #include -#include +#include +#include #include #include #include +#include #include #include @@ -94,6 +95,9 @@ PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); #include extern struct system_load_s system_load; @@ -143,7 +147,6 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); -static void tune_confirm(void); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -151,6 +154,7 @@ static int led_on(int led); static int led_off(int led); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -178,7 +182,7 @@ static int buzzer_init() buzzer = open("/dev/tone_alarm", O_WRONLY); if (buzzer < 0) { - fprintf(stderr, "[commander] Buzzer: open fail\n"); + warnx("Buzzer: open fail\n"); return ERROR; } @@ -196,12 +200,12 @@ static int led_init() leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { - fprintf(stderr, "[commander] LED: open fail\n"); + warnx("LED: open fail\n"); return ERROR; } if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - fprintf(stderr, "[commander] LED: ioctl fail\n"); + warnx("LED: ioctl fail\n"); return ERROR; } @@ -243,11 +247,12 @@ enum AUDIO_PATTERN { AUDIO_PATTERN_TETRIS = 5 }; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +{ /* Trigger alarm if going into any error state */ if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { + ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { ioctl(buzzer, TONE_SET_ALARM, 0); ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); } @@ -263,10 +268,47 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -void tune_confirm(void) { +void tune_confirm(void) +{ ioctl(buzzer, TONE_SET_ALARM, 3); } +void tune_error(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + return; + } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} + void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { @@ -292,7 +334,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // * but the smallest number by magnitude float can // * represent. Use -FLT_MAX to initialize the most // * negative number - + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; @@ -307,9 +349,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 0.0f, 1.0f, }; + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); } /* calibrate range */ @@ -329,14 +372,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const char axislabels[3] = { 'X', 'Y', 'Z'}; int axis_index = -1; - float *x = (float*)malloc(sizeof(float) * calibration_maxcount); - float *y = (float*)malloc(sizeof(float) * calibration_maxcount); - float *z = (float*)malloc(sizeof(float) * calibration_maxcount); + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); if (x == NULL || y == NULL || z == NULL) { warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - printf("x:%p y:%p z:%p\n", x, y, z); + warnx("x:%p y:%p z:%p\n", x, y, z); return; } @@ -345,22 +388,22 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) tune_confirm(); while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + calibration_counter < calibration_maxcount) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; /* user guidance */ if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { + axis_index < 3) { axis_index++; char buf[50]; - sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "Please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); tune_confirm(); - + axis_deadline += calibration_interval / 3; } @@ -372,7 +415,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // if ((axis_left / 1000) == 0 && axis_left > 0) { // char buf[50]; - // sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); // mavlink_log_info(mavlink_fd, buf); // } @@ -407,9 +450,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // } calibration_counter++; + } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] mag cal canceled"); + mavlink_log_info(mavlink_fd, "mag cal canceled"); break; } } @@ -440,45 +484,47 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to set scale / offsets for mag"); + close(fd); /* announce and set new offset */ if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + warnx("Setting X mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + warnx("Setting Y mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + warnx("Setting Z mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - fprintf(stderr, "[commander] Setting X mag scale failed!\n"); + warnx("Setting X mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - fprintf(stderr, "[commander] Setting Y mag scale failed!\n"); + warnx("Setting Y mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - fprintf(stderr, "[commander] Setting Z mag scale failed!\n"); + warnx("Setting Z mag scale failed!\n"); } /* auto-save to EEPROM */ int save_ret = param_save_default(); - if(save_ret != 0) { + + if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "[cmd] FAILED storing calibration"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); } - printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + char buf[52]; sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); @@ -488,7 +534,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + mavlink_log_info(mavlink_fd, "mag calibration done"); tune_confirm(); sleep(2); @@ -497,7 +543,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN)"); } /* disable calibration mode */ @@ -523,7 +569,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) /* set offsets to zero */ int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { + struct gyro_scale gscale_null = { 0.0f, 1.0f, 0.0f, @@ -531,8 +577,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) 0.0f, 1.0f, }; + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) warn("WARNING: failed to set scale / offsets for gyro"); + close(fd); while (calibration_counter < calibration_count) { @@ -546,9 +594,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); return; } } @@ -563,21 +612,15 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); - } - - if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); - } - - if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); } /* set offsets to actual value */ fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { + struct gyro_scale gscale = { gyro_offset[0], 1.0f, gyro_offset[1], @@ -585,28 +628,32 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[2], 1.0f, }; + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) warn("WARNING: failed to set scale / offsets for gyro"); + close(fd); /* auto-save to EEPROM */ int save_ret = param_save_default(); - if(save_ret != 0) { + + if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); } // char buf[50]; // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + mavlink_log_info(mavlink_fd, "gyro calibration done"); tune_confirm(); sleep(2); tune_confirm(); sleep(2); /* third beep by cal end routine */ + } else { - mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); } close(sub_sensor_combined); @@ -616,7 +663,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); + mavlink_log_info(mavlink_fd, "keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -638,9 +685,12 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) 0.0f, 1.0f, }; + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) warn("WARNING: failed to set scale / offsets for accel"); + close(fd); + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -652,48 +702,35 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) accel_offset[1] += raw.accelerometer_m_s2[1]; accel_offset[2] += raw.accelerometer_m_s2[2]; calibration_counter++; + } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); + mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); return; } } + accel_offset[0] = accel_offset[0] / calibration_count; accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { - + /* add the removed length from x / y to z, since we induce a scaling issue else */ - float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]); + float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); /* if length is correct, zero results here */ accel_offset[2] = accel_offset[2] + total_len; float scale = 9.80665f / total_len; - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); - } - - if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); } fd = open(ACCEL_DEVICE_PATH, 0); @@ -705,28 +742,32 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) accel_offset[2], scale, }; + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) warn("WARNING: failed to set scale / offsets for accel"); + close(fd); /* auto-save to EEPROM */ int save_ret = param_save_default(); - if(save_ret != 0) { + + if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); } //char buf[50]; - //sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] accel calibration done"); + mavlink_log_info(mavlink_fd, "accel calibration done"); tune_confirm(); sleep(2); tune_confirm(); sleep(2); /* third beep by cal end routine */ + } else { - mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); } /* exit accel calibration mode */ @@ -741,7 +782,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ - uint8_t result = MAV_RESULT_UNSUPPORTED; + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command handling */ tune_confirm(); @@ -751,99 +792,79 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { - case MAV_CMD_DO_SET_MODE: - { + case VEHICLE_CMD_DO_SET_MODE: { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } break; - case MAV_CMD_COMPONENT_ARM_DISARM: { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } - /* request to disarm */ + + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; /* request for an autopilot reboot */ - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { /* system may return here */ - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; - case PX4_CMD_CONTROLLER_SELECTION: { - bool changed = false; - if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { - current_vehicle_status->flag_control_rates_enabled = cmd->param1; - changed = true; - } - if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { - current_vehicle_status->flag_control_attitude_enabled = cmd->param2; - changed = true; - } - if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { - current_vehicle_status->flag_control_velocity_enabled = cmd->param3; - changed = true; - } - if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { - current_vehicle_status->flag_control_position_enabled = cmd->param4; - changed = true; - } - - if (changed) { - /* publish current state machine */ - state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); - } - } - // /* request to land */ -// case MAV_CMD_NAV_LAND: +// case VEHICLE_CMD_NAV_LAND: // { // //TODO: add check if landing possible // //TODO: add landing maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } } // break; // // /* request to takeoff */ -// case MAV_CMD_NAV_TAKEOFF: +// case VEHICLE_CMD_NAV_TAKEOFF: // { // //TODO: add check if takeoff possible // //TODO: add takeoff maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } // } // break; // /* preflight calibration */ - case MAV_CMD_PREFLIGHT_CALIBRATION: { + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { bool handled = false; /* gyro calibration */ @@ -852,17 +873,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration"); + mavlink_log_info(mavlink_fd, "starting gyro cal"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); + result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -872,17 +895,58 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration"); + mavlink_log_info(mavlink_fd, "starting mag cal"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + tune_confirm(); + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting trim cal"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished trim cal"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -892,32 +956,40 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration"); + mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); - mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); + mavlink_log_info(mavlink_fd, "CMD finished accel cal"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); + result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } /* none found */ if (!handled) { - //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); - result = MAV_RESULT_UNSUPPORTED; + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; - case MAV_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed) { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed"); + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + } else { // XXX move this to LOW PRIO THREAD of commander app @@ -927,57 +999,65 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* read all parameters from EEPROM to RAM */ int read_ret = param_load_default(); + if (read_ret == OK) { - //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "[cmd] OK loading params from"); + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "[cmd] OK no changes in"); + mavlink_log_info(mavlink_fd, "OK no changes in"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from"); + mavlink_log_info(mavlink_fd, "ERR loading params from"); mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { - mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); + mavlink_log_info(mavlink_fd, "ERR no param file named"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + + result = VEHICLE_CMD_RESULT_FAILED; } } else if (((int)(cmd->param1)) == 1) { /* write all parameters from RAM to EEPROM */ int write_ret = param_save_default(); + if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "[cmd] OK saved param file"); + mavlink_log_info(mavlink_fd, "OK saved param file"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "[cmd] ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { - mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to"); + mavlink_log_info(mavlink_fd, "ERR writing params to"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + + result = VEHICLE_CMD_RESULT_FAILED; } } else { mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } } break; - default: { + default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command rejection */ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -985,11 +1065,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* supported command handling stop */ - if (result == MAV_RESULT_FAILED || - result == MAV_RESULT_DENIED || - result == MAV_RESULT_UNSUPPORTED) { + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { ioctl(buzzer, TONE_SET_ALARM, 5); - } else if (result == MAV_RESULT_ACCEPTED) { + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_confirm(); } @@ -1010,7 +1091,7 @@ static void *orb_receive_loop(void *arg) //handles status information coming fr int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); struct subsystem_info_s info; - struct vehicle_status_s *vstatus = (struct vehicle_status_s*)arg; + struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; while (!thread_should_exit) { struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; @@ -1021,11 +1102,12 @@ static void *orb_receive_loop(void *arg) //handles status information coming fr /* got command */ orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - printf("Subsys changed: %d\n", (int)info.subsystem_type); + warnx("Subsys changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { vstatus->onboard_control_sensors_present |= info.subsystem_type; + } else { vstatus->onboard_control_sensors_present &= ~info.subsystem_type; } @@ -1033,6 +1115,7 @@ static void *orb_receive_loop(void *arg) //handles status information coming fr /* mark / unmark as enabled */ if (info.enabled) { vstatus->onboard_control_sensors_enabled |= info.subsystem_type; + } else { vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; } @@ -1040,6 +1123,7 @@ static void *orb_receive_loop(void *arg) //handles status information coming fr /* mark / unmark as ok */ if (info.ok) { vstatus->onboard_control_sensors_health |= info.subsystem_type; + } else { vstatus->onboard_control_sensors_health &= ~info.subsystem_type; } @@ -1090,6 +1174,7 @@ float battery_remaining_estimate_voltage(float voltage) param_get(bat_volt_full, &chemistry_voltage_full); param_get(bat_n_cells, &ncells); } + counter++; ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); @@ -1105,6 +1190,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -1113,7 +1199,7 @@ usage(const char *reason) * The daemon 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(). */ @@ -1125,7 +1211,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("commander already running\n"); + warnx("commander already running\n"); /* this is not an error */ exit(0); } @@ -1148,10 +1234,12 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tcommander is running\n"); + warnx("\tcommander is running\n"); + } else { - printf("\tcommander not started\n"); + warnx("\tcommander not started\n"); } + exit(0); } @@ -1168,8 +1256,10 @@ int commander_thread_main(int argc, char *argv[]) failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + param_t _param_sys_type = param_find("MAV_TYPE"); + /* welcome user */ - printf("[commander] I am in command now!\n"); + warnx("I am in command now!\n"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1177,17 +1267,17 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds\n"); } if (buzzer_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer\n"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); } /* make sure we are in preflight state */ @@ -1200,6 +1290,11 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; /* advertise to ORB */ @@ -1208,11 +1303,11 @@ int commander_thread_main(int argc, char *argv[]) state_machine_publish(stat_pub, ¤t_status, mavlink_fd); if (stat_pub < 0) { - printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); + warnx("ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); } - mavlink_log_info(mavlink_fd, "[commander] system is running"); + mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ pthread_attr_t subsystem_info_attr; @@ -1251,9 +1346,15 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; @@ -1264,10 +1365,16 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + /* subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1281,24 +1388,36 @@ int commander_thread_main(int argc, char *argv[]) uint64_t failsave_ll_start_time = 0; bool state_changed = true; + bool param_init_forced = true; + while (!thread_should_exit) { /* Get current values */ bool new_data; orb_check(sp_man_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } orb_check(sp_offboard_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); + if (new_data) { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); @@ -1307,7 +1426,55 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + } else { + warnx("ARMED, rejecting sys type change\n"); + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); battery_voltage = battery.voltage_v; @@ -1387,15 +1554,20 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!"); } low_voltage_counter++; @@ -1405,8 +1577,8 @@ int commander_thread_main(int argc, char *argv[]) else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!"); state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } @@ -1419,6 +1591,79 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + /* Check if last transition deserved an audio event */ // #warning This code depends on state that is no longer? maintained // #if 0 @@ -1450,7 +1695,7 @@ int commander_thread_main(int argc, char *argv[]) // // //// if(counter%10 == 0)//for testing only -//// printf("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only +//// warnx("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only // // } else { // gps_quality_good_counter = 0; @@ -1484,8 +1729,97 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* check if left stick is in lower left position --> switch to standby state */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + // /* + // * Check if manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // warnx("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1497,7 +1831,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1507,24 +1841,35 @@ int commander_thread_main(int argc, char *argv[]) stick_off_counter = 0; } } - //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { + /* check auto mode switch for correct mode */ + if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + /* enable guided mode */ + update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + + } } else { + /* center stick position, set SAS for all vehicle types */ update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME."); + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } current_status.rc_signal_cutting_off = false; @@ -1533,13 +1878,15 @@ int commander_thread_main(int argc, char *argv[]) } else { static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; @@ -1556,7 +1903,7 @@ int commander_thread_main(int argc, char *argv[]) } } - + /* End mode switch */ @@ -1570,8 +1917,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; @@ -1595,11 +1942,12 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_offboard_enabled = true; state_changed = true; tune_confirm(); - - mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); + + mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + } else { if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL"); + mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); state_changed = true; tune_confirm(); } @@ -1614,18 +1962,21 @@ int commander_thread_main(int argc, char *argv[]) update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } else if (!sp_offboard.armed && current_status.flag_system_armed) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } } else { static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!"); + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; @@ -1636,7 +1987,7 @@ int commander_thread_main(int argc, char *argv[]) tune_confirm(); /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) { + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { current_status.failsave_lowlevel = true; state_changed = true; } @@ -1654,7 +2005,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_preflight_gyro_calibration == false && current_status.flag_preflight_mag_calibration == false && current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ + /* All ok, no calibration going on, go to standby */ do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); } @@ -1682,11 +2033,11 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); - close(gps_sub); + close(global_position_sub); close(sensor_sub); close(cmd_sub); - printf("[commander] exiting..\n"); + warnx("exiting..\n"); fflush(stdout); thread_running = false; diff --git a/apps/commander/commander.h b/apps/commander/commander.h index bea67beada32..04b4e72ab31b 100644 --- a/apps/commander/commander.h +++ b/apps/commander/commander.h @@ -52,4 +52,7 @@ #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f +void tune_confirm(void); +void tune_error(void); + #endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 46793c89b0f3..d4e88b1464ac 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -50,7 +50,7 @@ #include "state_machine_helper.h" -static const char* system_state_txt[] = { +static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", "SYSTEM_STATE_STANDBY", "SYSTEM_STATE_GROUND_READY", @@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); // } else { // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); @@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); + warnx("EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: @@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); + warnx("EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: @@ -114,36 +114,40 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* prevent actuators from arming */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); + warnx("GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); break; case SYSTEM_STATE_PREFLIGHT: if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); } + break; case SYSTEM_STATE_REBOOT: if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT"); + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); } + break; case SYSTEM_STATE_STANDBY: @@ -152,7 +156,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* standby enforces disarmed */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: @@ -162,7 +166,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); + mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: @@ -172,7 +176,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); + mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: @@ -180,7 +184,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); + mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: @@ -188,7 +192,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); + mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); break; default: @@ -202,14 +206,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con publish_armed_status(current_status); ret = OK; } + if (invalid_state) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition"); + mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); ret = ERROR; } + return ret; } -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ /* publish the new state */ current_status->counter++; current_status->timestamp = hrt_absolute_time(); @@ -217,9 +224,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat /* assemble state vector based on flag values */ if (current_status->flag_control_rates_enabled) { current_status->onboard_control_sensors_present |= 0x400; + } else { current_status->onboard_control_sensors_present &= ~0x400; } + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; @@ -232,10 +241,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) { +void publish_armed_status(const struct vehicle_status_s *current_status) +{ struct actuator_armed_s armed; armed.armed = current_status->flag_system_armed; /* lock down actuators if required, only in HIL */ @@ -250,19 +260,19 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { */ void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); + warnx("EMERGENCY HANDLER\n"); /* Depending on the current state go to one of the error states */ if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - + // DO NOT abort mission //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); } else { - fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + warnx("Unknown system state: #%d\n", current_status->state_machine); } } @@ -272,7 +282,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); } else { - //global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); } } @@ -497,7 +507,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[commander] arming\n"); + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } } @@ -505,11 +515,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[commander] going standby\n"); + printf("[cmd] going standby\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] MISSION ABORT!\n"); + printf("[cmd] MISSION ABORT!\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } } @@ -518,54 +528,139 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->flag_control_manual_enabled = true; - /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = false; + } + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] manual mode\n"); + printf("[cmd] manual mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); } } void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + int old_mode = current_status->flight_mode; + int old_manual_control_mode = current_status->manual_control_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + current_status->flag_control_manual_enabled = true; + + if (old_mode != current_status->flight_mode || + old_manual_control_mode != current_status->manual_control_mode) { + printf("[cmd] att stabilized mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + state_machine_publish(status_pub, current_status, mavlink_fd); + } + + } +} + +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); + tune_error(); + return; + } if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] stabilized mode\n"); + printf("[cmd] position guided mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } } void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[commander] auto mode\n"); + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { - printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") + + } else { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } + /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { /* only arm in standby state */ @@ -573,7 +668,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; - printf("[commander] arming due to command request\n"); + printf("[cmd] arming due to command request\n"); } } @@ -583,26 +678,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; - printf("[commander] disarming due to command request\n"); + printf("[cmd] disarming due to command request\n"); } } - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.") - } - /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); ret = ERROR; } @@ -627,7 +710,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { printf("system will reboot\n"); - //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + mavlink_log_critical(mavlink_fd, "Rebooting.."); + usleep(200000); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); ret = 0; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index c4d1b78a5706..2f2ccc7298e5 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c */ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +/** + * Handle state machine if mode switch is guided + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + /** * Handle state machine if mode switch is auto * diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 695653714500..03da35a0a51c 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -293,17 +293,20 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + _guide.update(_pos, _att, _posCmd, _lastPosCmd); + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_STABILIZED) { - _stabilization.update( - _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { - } else if (_status.state_machine == SYSTEM_STATE_AUTO) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -325,10 +328,23 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _stabilization.update( + _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; + } } // update all publications diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index c2aed9b54671..2a7bfe3d734a 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -148,9 +148,7 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER hrt_init(); -#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -158,27 +156,21 @@ __EXPORT int nsh_archinitialize(void) #endif /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); // initial LED state drv_led_start(); @@ -233,7 +225,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN10); stm32_configgpio(GPIO_ADC1_IN11); - //stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available? + stm32_configgpio(GPIO_ADC1_IN12); //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards return OK; diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 78c061819439..fe9b281f67ad 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include #include #include -// #include +#include +#include #include #include -#include #include #include @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } @@ -396,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ @@ -419,9 +429,6 @@ HIL::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status */ - // up_pwm_servo_arm(aa.armed); } } diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 3b835904f6cc..430d18c6d53f 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ #include #include +#include #include #include @@ -97,6 +99,7 @@ class PX4FMU : public device::CDev int _t_actuators; int _t_armed; orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,6 +165,7 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_armed(-1), _t_outputs(0), + _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -319,6 +323,13 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -336,8 +347,16 @@ PX4FMU::task_main() if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); @@ -364,20 +383,39 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } } @@ -394,6 +432,7 @@ PX4FMU::task_main() } ::close(_t_actuators); + ::close(_t_actuators_effective); ::close(_t_armed); /* make sure servos are off */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c0a20c133ed3..2c2c236caea9 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -70,9 +71,13 @@ #include #include #include +#include +#include #include +#include #include +#include #include #include @@ -90,10 +95,18 @@ class PX4IO : public device::CDev virtual int ioctl(file *filp, int cmd, unsigned long arg); + /** + * Set the PWM via serial update rate + * @warning this directly affects CPU load + */ + int set_pwm_rate(int hz); + bool dump_one; private: + // XXX static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; + unsigned _update_rate; ///< serial send rate in Hz int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -105,8 +118,13 @@ class PX4IO : public device::CDev int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + int _t_vstatus; ///< system / vehicle status + vehicle_status_s _vstatus; ///< overall system state orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values @@ -191,13 +209,16 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), dump_one(false), + _update_rate(50), _serial_fd(-1), _io_stream(nullptr), _task(-1), _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), + _t_vstatus(-1), _t_outputs(-1), _mix_buf(nullptr), _mix_buf_len(0), @@ -205,7 +226,7 @@ PX4IO::PX4IO() : _relays(0), _switch_armed(false), _send_needed(false), - _config_needed(false) + _config_needed(true) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -253,7 +274,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -279,6 +300,17 @@ PX4IO::init() return OK; } +int +PX4IO::set_pwm_rate(int hz) +{ + if (hz > 0 && hz <= 400) { + _update_rate = hz; + return OK; + } else { + return -EINVAL; + } +} + void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -288,7 +320,8 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - debug("starting"); + log("starting"); + unsigned update_rate_in_ms; /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); @@ -328,12 +361,20 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); /* 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, 20); /* XXX 50Hz hardcoded for now */ + update_rate_in_ms = 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 */ + _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -348,13 +389,15 @@ PX4IO::task_main() _to_battery = -1; /* poll descriptor */ - pollfd fds[3]; + pollfd fds[4]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; fds[1].fd = _t_actuators; fds[1].events = POLLIN; fds[2].fd = _t_armed; fds[2].events = POLLIN; + fds[3].fd = _t_vstatus; + fds[3].events = POLLIN; debug("ready"); @@ -405,12 +448,11 @@ PX4IO::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } - } - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); + _send_needed = true; + } } /* send a config packet to IO if required */ @@ -427,6 +469,12 @@ PX4IO::task_main() _mix_buf = nullptr; _mix_buf_len = 0; } + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } } unlock(); @@ -565,20 +613,27 @@ PX4IO::io_send() cmd.f2i_magic = F2I_MAGIC; /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { cmd.output_control[i] = _outputs.output[i]; - + } /* publish as we send */ + _outputs.timestamp = hrt_absolute_time(); /* XXX needs to be based off post-mix values from the IO side */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); /* update relays */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - /* armed and not locked down */ + /* armed and not locked down -> arming ok */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); - + /* indicate that full autonomous position control / vector flight mode is available */ + cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; + /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ + cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; + /* set desired PWM output rate */ + cmd.servo_rate = _update_rate; + ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); if (ret) @@ -593,6 +648,47 @@ PX4IO::config_send() cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + int val; + + /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ + param_get(param_find("RC_MAP_ROLL"), &val); + cfg.rc_map[0] = val; + param_get(param_find("RC_MAP_PITCH"), &val); + cfg.rc_map[1] = val; + param_get(param_find("RC_MAP_YAW"), &val); + cfg.rc_map[2] = val; + param_get(param_find("RC_MAP_THROTTLE"), &val); + cfg.rc_map[3] = val; + + /* set the individual channel properties */ + char nbuf[16]; + float float_val; + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MIN", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_min[i] = float_val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_TRIM", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_trim[i] = float_val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MAX", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_max[i] = float_val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_REV", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_rev[i] = float_val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_DZ", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_dz[i] = float_val; + } + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); if (ret) @@ -779,7 +875,7 @@ test(void) void monitor(void) { - unsigned cancels = 4; + unsigned cancels = 3; printf("Hit three times to exit monitor mode\n"); for (;;) { @@ -801,6 +897,7 @@ monitor(void) g_dev->dump_one = true; } } + } int @@ -822,6 +919,16 @@ px4io_main(int argc, char *argv[]) errx(1, "driver init failed"); } + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 + 1) { + g_dev->set_pwm_rate(atoi(argv[2 + 1])); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + } + exit(0); } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 334b95226fb2..b012448a23c3 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -112,8 +112,9 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc } int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - struct vehicle_rates_setpoint_s *rates_sp) + const struct vehicle_attitude_s *att, + const float speed_body[], + struct vehicle_rates_setpoint_s *rates_sp) { static int counter = 0; static bool initialized = false; @@ -125,8 +126,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att static PID_t pitch_controller; - if(!initialized) - { + if (!initialized) { parameters_init(&h); parameters_update(&h, &p); pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller @@ -145,12 +145,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; - rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); + + /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ + float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); + /* set pitch plus feedforward roll compensation */ + rates_sp->pitch = pid_calculate(&pitch_controller, + att_sp->pitch_body + pitch_sp_rollcompensation, + att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ - //TODO + rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) + / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); + +// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/apps/fixedwing_att_control/fixedwing_att_control_att.h index ca7c14b43c5b..600e35b898b2 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.h +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.h @@ -41,9 +41,11 @@ #include #include #include +#include int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - struct vehicle_rates_setpoint_s *rates_sp); + const struct vehicle_attitude_s *att, + const float speed_body[], + struct vehicle_rates_setpoint_s *rates_sp); #endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 38f58ac33a7e..aa9db6d5287b 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -58,40 +58,16 @@ #include #include #include +#include #include #include #include #include +#include #include - #include #include -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - /* Prototypes */ /** * Deamon management function. @@ -117,118 +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); - /* welcome user */ - printf("[fixedwing att_control] started\n"); + /* 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)); - /* 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 manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); + /* 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 }; -// static struct debug_key_value_s debug_output; -// memset(&debug_output, 0, sizeof(debug_output)); + while (!thread_should_exit) { + /* wait for a sensor update, check for exit condition every 500 ms */ + poll(&fds, 1, 500); - /* output structs */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); + /* 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); - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; + 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_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); -// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); -// debug_output.key[0] = '1'; + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* subscribe */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; - /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + /* control */ - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); + 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); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { + if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - /* Control */ + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (vstatus.rc_signal_lost) { - if (vstatus.state_machine == SYSTEM_STATE_AUTO) { - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &rates_sp); + /* put plane into loiter */ + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; - /* Attitude Rate Control */ + /* 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; + + // XXX disable yaw control, loiter + + } 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.timestamp = hrt_absolute_time(); + + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - //REMOVEME XXX - actuators.control[3] = 0.7f; + /* 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; + } - // debug_output.value = rates_sp.pitch; - // orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { + } 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[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; + } } + } - /* publish output */ + /* 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; + 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(att_sub); + close(actuator_pub); + close(rates_pub); - fflush(stdout); - exit(0); + fflush(stdout); + exit(0); - return 0; + return 0; } @@ -239,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); } @@ -268,7 +338,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_att_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; @@ -283,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); } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index b81d50168b12..87b6e925c154 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler + * Author: Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,8 @@ /** * @file fixedwing_att_control_rate.c * Implementation of a fixed wing attitude controller. + * + * @author Thomas Gubler */ #include @@ -59,9 +61,33 @@ #include #include - - - +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* feedforward compensation */ +PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ struct fw_rate_control_params { float rollrate_p; @@ -73,7 +99,7 @@ struct fw_rate_control_params { float yawrate_p; float yawrate_i; float yawrate_awu; - + float pitch_thr_ff; }; struct fw_rate_control_param_handles { @@ -86,7 +112,7 @@ struct fw_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_awu; - + param_t pitch_thr_ff; }; @@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); return OK; } @@ -124,14 +151,14 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_awu, &(p->yawrate_awu)); - + param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); return OK; } int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators) + const float rates[], + struct actuator_controls_s *actuators) { static int counter = 0; static bool initialized = false; @@ -147,8 +174,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); - if(!initialized) - { + if (!initialized) { parameters_init(&h); parameters_update(&h, &p); pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller @@ -167,12 +193,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } - /* Roll Rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); - - - actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... - actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now + /* roll rate (PI) */ + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + /* pitch rate (PI) */ + actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ + actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; + /* yaw rate (PI) */ + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.h b/apps/fixedwing_att_control/fixedwing_att_control_rate.h index d394c3dacd1b..500e3e19710a 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.h +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.h @@ -42,7 +42,7 @@ #include int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators); + const float rates[], + struct actuator_controls_s *actuators); #endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile deleted file mode 100644 index 02d4463dd05b..000000000000 --- a/apps/fixedwing_control/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# 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. -# -############################################################################ - -# -# fixedwing_control Application -# - -APPNAME = fixedwing_control -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -CSRCS = fixedwing_control.c - -include $(APPDIR)/mk/app.mk diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c deleted file mode 100644 index e04033481937..000000000000 --- a/apps/fixedwing_control/fixedwing_control.c +++ /dev/null @@ -1,566 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov - * Modifications: Doug Weibel - * - * 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 fixedwing_control.c - * Implementation of a fixed wing attitude and position controller. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - -struct fw_att_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float rollrate_lim; - float roll_p; - float roll_lim; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float pitchrate_lim; - float pitch_p; - float pitch_lim; -}; - -struct fw_att_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t rollrate_lim; - param_t roll_p; - param_t roll_lim; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t pitchrate_lim; - param_t pitch_p; - param_t pitch_lim; -}; - - -// TO_DO - Navigation control will be moved to a separate app -// Attitude control will just handle the inner angle and rate loops -// to control pitch and roll, and turn coordination via rudder and -// possibly throttle compensation for battery voltage sag. - -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); - -struct fw_pos_control_params { - float heading_p; - float heading_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t heading_lim; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int att_parameters_init(struct fw_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); - -/** - * Initialize all parameter handles and values - * - */ -static int pos_parameters_init(struct fw_pos_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - - -/** - * The fixed wing control main thread. - * - * The main loop executes continously and calculates the control - * response. - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -int fixedwing_control_thread_main(int argc, char *argv[]) -{ - /* 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; - } - } - - /* welcome user */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - 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); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - /* 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 */ - 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)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - - uint64_t last_run = 0; - uint64_t last_run_pos = 0; - - bool global_sp_updated_set_once = false; - - struct fw_att_control_params p; - struct fw_att_control_param_handles h; - - struct fw_pos_control_params ppos; - struct fw_pos_control_param_handles hpos; - - /* initialize the pid controllers */ - att_parameters_init(&h); - att_parameters_update(&h, &p); - - pos_parameters_init(&hpos); - pos_parameters_update(&hpos, &ppos); - -// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere - PID_t roll_rate_controller; - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, - p.rollrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t roll_angle_controller; - pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, - p.roll_lim,PID_MODE_DERIVATIV_NONE); - - PID_t pitch_rate_controller; - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, - p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t pitch_angle_controller; - pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, - p.pitch_lim,PID_MODE_DERIVATIV_NONE); - - PID_t heading_controller; - pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit - - // 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); - - // This is the top of the main loop - while(!thread_should_exit) { - - struct pollfd fds[1] = { - { .fd = att_sub, .events = POLLIN }, - }; - int ret = poll(fds, 1, 1000); - - 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 { - - // FIXME SUBSCRIBE - if (loopcounter % 100 == 0) { - att_parameters_update(&h, &p); - pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, - p.rollrate_awu, p.rollrate_lim); - pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, - 0.0f, p.roll_lim); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, - p.pitchrate_awu, p.pitchrate_lim); - pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, - 0.0f, p.pitch_lim); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); -//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", -//p.rollrate_p, p.rollrate_i, p.rollrate_lim); - } - - /* 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); - // XXX update to switch between external attitude reference and the - // attitude calculated here - - char name[10]; - - 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 = 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; - } - - if (bearing_error > M_PI_F) { - 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); - - /* 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.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(); - - if (verbose && (loopcounter % 20 == 0)) { - printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); - } - - // actuator control[0] is aileron (or elevon roll control) - // Commanded roll rate from P controller on roll angle - float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, - att.roll, 0.0f, deltaTpos); - // actuator control from PI controller on roll rate - actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, - att.rollspeed, 0.0f, deltaTpos); - - // actuator control[1] is elevator (or elevon pitch control) - // Commanded pitch rate from P controller on pitch angle - float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, - att.pitch, 0.0f, deltaTpos); - // actuator control from PI controller on pitch rate - actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, - att.pitchspeed, 0.0f, deltaTpos); - - // actuator control[3] is throttle - actuators.control[3] = att_sp.thrust; - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - } - } - - printf("[fixedwing_control] exiting.\n"); - thread_running = false; - - return 0; -} - -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_spawn("fixedwing_control", - SCHED_DEFAULT, - 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); -} - -static int att_parameters_init(struct fw_att_control_param_handles *h) -{ - /* PID parameters */ - - h->rollrate_p = param_find("FW_ROLLRATE_P"); - h->rollrate_i = param_find("FW_ROLLRATE_I"); - h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); - h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); - h->roll_p = param_find("FW_ROLL_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitchrate_p = param_find("FW_PITCHRATE_P"); - h->pitchrate_i = param_find("FW_PITCHRATE_I"); - h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); - h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->roll_p, &(p->roll_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - 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/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index a70b9bd30d0b..71c78f5b868a 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -57,24 +57,31 @@ #include #include #include +#include #include #include #include +#include #include /* * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - +PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ struct fw_pos_control_params { float heading_p; + float headingr_p; + float headingr_i; + float headingr_lim; float xtrack_p; float altitude_p; float roll_lim; @@ -83,11 +90,13 @@ struct fw_pos_control_params { struct fw_pos_control_param_handles { param_t heading_p; + param_t headingr_p; + param_t headingr_i; + param_t headingr_lim; param_t xtrack_p; param_t altitude_p; param_t roll_lim; param_t pitch_lim; - }; @@ -99,8 +108,8 @@ struct planned_path_segments_s { double end_lon; float radius; // Radius of arc float arc_start_bearing; // Bearing from center to start of arc - float arc_sweep; // Angle (radians) swept out by arc around center. - // Positive for clockwise, negative for counter-clockwise + float arc_sweep; // Angle (radians) swept out by arc around center. + // Positive for clockwise, negative for counter-clockwise }; @@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - + h->heading_p = param_find("FW_HEAD_P"); + h->headingr_p = param_find("FW_HEADR_P"); + h->headingr_i = param_find("FW_HEADR_I"); + h->headingr_lim = param_find("FW_HEADR_LIM"); + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } @@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h) static int 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->headingr_p, &(p->headingr_p)); + param_get(h->headingr_i, &(p->headingr_i)); + param_get(h->headingr_lim, &(p->headingr_lim)); param_get(h->xtrack_p, &(p->xtrack_p)); param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); @@ -162,172 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc int fixedwing_pos_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_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct vehicle_global_position_s start_pos; // Temporary variable, replace with - memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available - struct vehicle_global_position_setpoint_s global_setpoint; - memset(&global_setpoint, 0, sizeof(global_setpoint)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct crosstrack_error_s xtrack_err; - memset(&xtrack_err, 0, sizeof(xtrack_err)); - - /* output structs */ - struct vehicle_attitude_setpoint_s attitude_setpoint; - memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); - - /* publish attitude setpoint */ - attitude_setpoint.roll_body = 0.0f; - attitude_setpoint.pitch_body = 0.0f; - attitude_setpoint.yaw_body = 0.0f; - orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); - - /* subscribe */ - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - /* Setup of loop */ - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - bool global_sp_updated_set_once = false; - - float psi_track = 0.0f; - - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - static int counter = 0; - static bool initialized = false; - - static struct fw_pos_control_params p; - static struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t altitude_controller; - - if(!initialized) - { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); - initialized = true; - } + /* welcome user */ + printf("[fixedwing pos control] started\n"); + + /* declare and safely initialize all structs */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + struct vehicle_global_position_s start_pos; // Temporary variable, replace with + memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available + struct vehicle_global_position_setpoint_s global_setpoint; + memset(&global_setpoint, 0, sizeof(global_setpoint)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct crosstrack_error_s xtrack_err; + memset(&xtrack_err, 0, sizeof(xtrack_err)); + struct parameter_update_s param_update; + memset(¶m_update, 0, sizeof(param_update)); + + /* output structs */ + struct vehicle_attitude_setpoint_s attitude_setpoint; + memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); + + /* publish attitude setpoint */ + attitude_setpoint.roll_body = 0.0f; + attitude_setpoint.pitch_body = 0.0f; + attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.thrust = 0.0f; + orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); + + /* subscribe */ + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + + /* Setup of loop */ + struct pollfd fds[2] = { + { .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN } + }; + bool global_sp_updated_set_once = false; + + float psi_track = 0.0f; + + int counter = 0; + + struct fw_pos_control_params p; + struct fw_pos_control_param_handles h; + + PID_t heading_controller; + PID_t heading_rate_controller; + PID_t offtrack_controller; + PID_t altitude_controller; + + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + + /* error and performance monitoring */ + perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); + perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); + + while (!thread_should_exit) { + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(fw_err_perf); + + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); - /* load new parameters with lower rate */ - if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value } - /* Check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* Load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(pos_updated) - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - if(global_sp_updated) { - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", (double)psi_track); - } + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { - /* Control */ + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - /* Simple Horizontal Control */ - if(global_sp_updated_set_once) - { -// if (counter % 100 == 0) -// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + /* check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); - if(!(distance_res != OK || xtrack_err.past_end)) { + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } - if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) - delta_psi_c = 60.0f*M_DEG_TO_RAD_F; + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) - delta_psi_c = -60.0f*M_DEG_TO_RAD_F; + printf("next wp direction: %0.4f\n", (double)psi_track); + } - float psi_c = psi_track + delta_psi_c; + /* Simple Horizontal Control */ + if (global_sp_updated_set_once) { + // if (counter % 100 == 0) + // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - float psi_e = psi_c - att.yaw; + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); + // XXX what is xtrack_err.past_end? + if (distance_res == OK /*&& !xtrack_err.past_end*/) { - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance -// if (counter % 100 == 0) -// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { - if (counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); - } + float psi_c = psi_track + delta_psi_c; + float psi_e = psi_c - att.yaw; - } + /* wrap difference back onto -pi..pi range */ + psi_e = _wrap_pi(psi_e); - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { + if (verbose) { + printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); + printf("delta_psi_c %.4f ", (double)delta_psi_c); + printf("psi_c %.4f ", (double)psi_c); + printf("att.yaw %.4f ", (double)att.yaw); + printf("psi_e %.4f ", (double)psi_e); + } - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + /* calculate roll setpoint, do this artificially around zero */ + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following + float psi_rate_c = delta_psi_rate_c + psi_rate_track; - } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + /* limit turn rate */ + if (psi_rate_c > p.headingr_lim) { + psi_rate_c = p.headingr_lim; + + } else if (psi_rate_c < -p.headingr_lim) { + psi_rate_c = -p.headingr_lim; + } + + float psi_rate_e = psi_rate_c - att.yawspeed; + + // XXX sanity check: Assume 10 m/s stall speed and no stall condition + float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - counter++; + if (ground_speed < 10.0f) { + ground_speed = 10.0f; + } + + float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g + + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + + if (verbose) { + printf("psi_rate_c %.4f ", (double)psi_rate_c); + printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); + printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); + } + + if (verbose && counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c); + + } else { + if (verbose && counter % 100 == 0) + printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + } + + /* Very simple Altitude Control */ + if (pos_updated) { + + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + + } + + // XXX need speed control + attitude_setpoint.thrust = 0.7f; + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + + counter++; + + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } } + } - printf("[fixedwing_pos_control] exiting.\n"); - thread_running = false; + printf("[fixedwing_pos_control] exiting.\n"); + thread_running = false; - close(attitude_setpoint_pub); + close(attitude_setpoint_pub); - fflush(stdout); - exit(0); + fflush(stdout); + exit(0); - return 0; + return 0; } @@ -338,6 +421,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n"); exit(1); } @@ -367,7 +451,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; @@ -382,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tfixedwing_pos_control is running\n"); + } else { printf("\tfixedwing_pos_control not started\n"); } + exit(0); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 81b907bcdb36..2ec459ddc7d7 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -156,12 +156,15 @@ set_hil_on_off(bool hil_enabled) if (baudrate < 19200) { /* 10 Hz */ hil_rate_interval = 100; + } else if (baudrate < 38400) { /* 10 Hz */ hil_rate_interval = 100; + } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; + } else { /* 200 Hz */ hil_rate_interval = 5; @@ -189,17 +192,38 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode = 0; /* set mode flags independent of system state */ + + /* HIL */ + if (v_status.flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* manual input */ if (v_status.flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.flag_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + /* attitude or rate control */ + if (v_status.flag_control_attitude_enabled || + v_status.flag_control_rates_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + /* vector control */ + if (v_status.flag_control_velocity_enabled || + v_status.flag_control_position_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + + /* autonomous mode */ + if (v_status.state_machine == SYSTEM_STATE_AUTO) { + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ if (armed.armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } @@ -210,9 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) v_status.flag_preflight_mag_calibration || v_status.flag_preflight_accel_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; + } else { *mavlink_state = MAV_STATE_UNINIT; } + break; case SYSTEM_STATE_STANDBY: @@ -225,17 +251,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) case SYSTEM_STATE_MANUAL: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case SYSTEM_STATE_STABILIZED: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; break; case SYSTEM_STATE_AUTO: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case SYSTEM_STATE_MISSION_ABORT: @@ -267,41 +290,48 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m int ret = OK; switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_RAW_IMU: - /* sensor sub triggers RAW IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_ATTITUDE: - /* attitude sub triggers attitude */ - orb_set_interval(subs->att_sub, min_interval); - break; - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: - /* actuator_outputs triggers this message */ - orb_set_interval(subs->act_0_sub, min_interval); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - orb_set_interval(subs->act_3_sub, min_interval); - orb_set_interval(subs->actuators_sub, min_interval); - break; - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); - break; - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); - break; - default: - /* not found */ - ret = ERROR; - break; + case MAVLINK_MSG_ID_SCALED_IMU: + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_HIGHRES_IMU: + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_RAW_IMU: + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(subs->att_sub, min_interval); + break; + + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + orb_set_interval(subs->debug_key_value, min_interval); + break; + + default: + /* not found */ + ret = ERROR; + break; } return ret; @@ -443,7 +473,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } -void +void mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) { write(uart, ch, (size_t)(sizeof(uint8_t) * length)); @@ -452,7 +482,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t* mavlink_get_channel_status(uint8_t channel) +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_status[channel]; @@ -461,7 +491,7 @@ mavlink_status_t* mavlink_get_channel_status(uint8_t channel) /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_buffer[channel]; @@ -470,31 +500,35 @@ mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) void mavlink_update_system(void) { static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + initialized = true; } /* update system and component id */ int32_t system_id; param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } - + int32_t system_type; param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } @@ -520,8 +554,10 @@ int mavlink_thread_main(int argc, char *argv[]) switch (ch) { case 'b': baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) errx(1, "invalid baud rate '%s'", optarg); + break; case 'd': @@ -542,6 +578,7 @@ int mavlink_thread_main(int argc, char *argv[]) } struct termios uart_config_original; + bool usb_uart; /* print welcome text */ @@ -555,6 +592,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* default values for arguments */ uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + if (uart < 0) err(1, "could not open %s", device_name); @@ -585,6 +623,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + } else if (baudrate >= 115200) { /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); @@ -595,6 +634,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); @@ -607,6 +647,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); @@ -659,6 +700,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.errors_count4); lowspeed_counter = 0; } + lowspeed_counter++; /* sleep quarter the time */ @@ -675,6 +717,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* sleep quarter the time */ usleep(25000); + if (baudrate > 57600) { mavlink_pm_queued_send(); } @@ -686,6 +729,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); + if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); } @@ -712,8 +756,8 @@ static void usage() { fprintf(stderr, "usage: mavlink start [-d ] [-b ]\n" - " mavlink stop\n" - " mavlink status\n"); + " mavlink stop\n" + " mavlink status\n"); exit(1); } @@ -737,15 +781,18 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, mavlink_thread_main, - (const char**)argv); + (const char **)argv); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; + while (thread_running) { usleep(200000); + printf("."); } + warnx("terminated."); exit(0); } @@ -753,6 +800,7 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { errx(0, "running"); + } else { errx(1, "not running"); } diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index bf7c5354ccd5..270ec1727423 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -75,7 +75,7 @@ extern mavlink_system_t mavlink_system; */ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); -mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); +mavlink_status_t *mavlink_get_channel_status(uint8_t chan); +mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h index 95790db93463..1eb8c32e9e7f 100644 --- a/apps/mavlink/mavlink_hil.h +++ b/apps/mavlink/mavlink_hil.h @@ -50,7 +50,7 @@ extern orb_advert_t pub_hil_attitude; * Enable / disable Hardware in the Loop simulation mode. * * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the + * @return OK if the HIL state changed, ERROR if the * requested change could not be made or was * redundant. */ diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c index 73d59e76fc4c..ed65fb883ce7 100644 --- a/apps/mavlink/mavlink_log.c +++ b/apps/mavlink/mavlink_log.c @@ -43,39 +43,47 @@ #include #include "mavlink_log.h" - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +{ + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); } - -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +{ return lb->count == (int)lb->size; } - -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { - return lb->count == 0; + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +{ + return lb->count == 0; } - -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); - if (mavlink_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - } else { - ++lb->count; - } + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +{ + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } } - -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { - if (!mavlink_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - return 0; - } else { - return 1; - } + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +{ + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + + } else { + return 1; + } } diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index cb6fd9d98c5c..62e6f7ca074d 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -82,26 +82,26 @@ #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); struct mavlink_logmessage { - char text[51]; - unsigned char severity; + char text[51]; + unsigned char severity; }; struct mavlink_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct mavlink_logmessage *elems; + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; }; - + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); - + int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); - + void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); - + int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); #endif diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 9d9b9914a79a..819f3441bbb6 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -75,7 +75,7 @@ void mavlink_pm_callback(void *arg, param_t param); void mavlink_pm_callback(void *arg, param_t param) { mavlink_pm_send_param(param); - usleep(*(unsigned int*)arg); + usleep(*(unsigned int *)arg); } void mavlink_pm_send_all_params(unsigned int delay) @@ -90,6 +90,7 @@ int mavlink_pm_queued_send() mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); mavlink_param_queue_index++; return 0; + } else { return 1; } @@ -105,7 +106,7 @@ int mavlink_pm_send_param_for_index(uint16_t index) return mavlink_pm_send_param(param_for_index(index)); } -int mavlink_pm_send_param_for_name(const char* name) +int mavlink_pm_send_param_for_name(const char *name) { return mavlink_pm_send_param(param_find(name)); } @@ -123,16 +124,19 @@ int mavlink_pm_send_param(param_t param) param_type_t type = param_type(param); /* copy parameter name */ strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - + /* * Map onboard parameter type to MAVLink type, * endianess matches (both little endian) */ uint8_t mavlink_type; + if (type == PARAM_TYPE_INT32) { mavlink_type = MAVLINK_TYPE_INT32_T; + } else if (type == PARAM_TYPE_FLOAT) { mavlink_type = MAVLINK_TYPE_FLOAT; + } else { mavlink_type = MAVLINK_TYPE_FLOAT; } @@ -143,7 +147,10 @@ int mavlink_pm_send_param(param_t param) */ int ret; - if ((ret = param_get(param, &val_buf)) != OK) return ret; + + if ((ret = param_get(param, &val_buf)) != OK) { + return ret; + } mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, @@ -161,13 +168,13 @@ int mavlink_pm_send_param(param_t param) void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); } break; - case MAVLINK_MSG_ID_PARAM_SET: { + case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ @@ -177,7 +184,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; @@ -188,6 +195,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[mavlink pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); + } else { /* set and send parameter */ param_set(param, &(mavlink_param_set.param_value)); @@ -197,25 +205,26 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } break; - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } + if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + /* when no index is given, loop through string ids and compare them */ + if (mavlink_param_request_read.param_index == -1) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + mavlink_pm_send_param_for_name(name); + + } else { + /* when index is >= 0, send this parameter again */ + mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); } + } } break; } diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index 146e9e15c479..b1e38bcc852f 100644 --- a/apps/mavlink/mavlink_parameters.h +++ b/apps/mavlink/mavlink_parameters.h @@ -48,7 +48,7 @@ #include /** - * Handle parameter related messages. + * Handle parameter related messages. */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); @@ -84,14 +84,14 @@ int mavlink_pm_send_param_for_index(uint16_t index); * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ -int mavlink_pm_send_param_for_name(const char* name); +int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); +int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed642..79452f515e0f 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -106,7 +106,7 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ @@ -138,6 +138,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -162,6 +163,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -191,6 +193,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* create command */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -214,6 +217,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -230,7 +234,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -238,33 +242,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -276,6 +284,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -298,6 +307,26 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if (mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -305,6 +334,7 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; + /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); @@ -357,12 +387,14 @@ handle_message(mavlink_message_t *msg) if (rc_pub == 0) { rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + } else { orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); } if (mc_pub == 0) { mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + } else { orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); } @@ -377,7 +409,7 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; uint8_t ch; @@ -421,8 +453,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c index 50282a9e309a..8064febc47a9 100644 --- a/apps/mavlink/missionlib.c +++ b/apps/mavlink/missionlib.c @@ -106,6 +106,7 @@ mavlink_missionlib_send_gcs_string(const char *string) mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); return mavlink_missionlib_send_message(&msg); + } else { return 1; } @@ -144,12 +145,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -160,12 +164,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { @@ -175,15 +182,18 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } - + mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); diff --git a/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h index 3439c185dde2..c2ca735b3bac 100644 --- a/apps/mavlink/missionlib.h +++ b/apps/mavlink/missionlib.h @@ -48,5 +48,5 @@ extern int mavlink_missionlib_send_message(mavlink_message_t *msg); extern int mavlink_missionlib_send_gcs_string(const char *string); extern uint64_t mavlink_missionlib_get_system_timestamp(void); extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e6ec77bf6672..f5253ea358ae 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -90,9 +90,8 @@ static uint64_t last_sensor_timestamp; static void *uorb_receive_thread(void *arg); -struct listener -{ - void (*callback)(struct listener *l); +struct listener { + void (*callback)(struct listener *l); int *subp; uintptr_t arg; }; @@ -185,14 +184,14 @@ l_sensor_combined(struct listener *l) if (gcs_link) mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1],raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + raw.gyro_rad_s[1], raw.gyro_rad_s[2], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1], raw.magnetometer_ga[2], + raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); sensors_raw_counter++; } @@ -209,13 +208,13 @@ l_vehicle_attitude(struct listener *l) if (gcs_link) /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); attitude_counter++; } @@ -291,21 +290,21 @@ l_input_rc(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - + if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, - 0, - (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, - (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, - (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, - (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, - (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, - (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, - (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, - (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, - 255); + rc_raw.timestamp / 1000, + 0, + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -317,7 +316,7 @@ l_global_position(struct listener *l) uint64_t timestamp = global_pos.timestamp; int32_t lat = global_pos.lat; int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt*1000); + int32_t alt = (int32_t)(global_pos.alt * 1000); int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); int16_t vx = (int16_t)(global_pos.vx * 100.0f); int16_t vy = (int16_t)(global_pos.vy * 100.0f); @@ -343,16 +342,16 @@ l_local_position(struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - + if (gcs_link) mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); } void @@ -364,16 +363,17 @@ l_global_position_setpoint(struct listener *l) orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + if (global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, - global_sp.lat, - global_sp.lon, - global_sp.altitude, - global_sp.yaw); + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); } void @@ -386,11 +386,11 @@ l_local_position_setpoint(struct listener *l) if (gcs_link) mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); } void @@ -403,11 +403,11 @@ l_attitude_setpoint(struct listener *l) if (gcs_link) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, - att_sp.timestamp/1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); } void @@ -420,11 +420,11 @@ l_vehicle_rates_setpoint(struct listener *l) if (gcs_link) mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, - rates_sp.timestamp/1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); + rates_sp.timestamp / 1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); } void @@ -444,18 +444,18 @@ l_actuator_outputs(struct listener *l) if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number */, - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7]); + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -468,72 +468,75 @@ l_actuator_outputs(struct listener *l) if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + } else { + + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ float rudder, throttle; - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; } mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, - mavlink_mode, - 0); + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 600.0f, + (act_outputs.output[1] - 1500.0f) / 600.0f, + rudder, + throttle, + (act_outputs.output[4] - 1500.0f) / 600.0f, + (act_outputs.output[5] - 1500.0f) / 600.0f, + (act_outputs.output[6] - 1500.0f) / 600.0f, + (act_outputs.output[7] - 1500.0f) / 600.0f, + mavlink_mode, + 0); } } } @@ -555,39 +558,39 @@ l_manual_control_setpoint(struct listener *l) if (gcs_link) mavlink_msg_manual_control_send(MAVLINK_COMM_0, - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); } void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_controls_effective_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); + "eff ctrl0 ", + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); + "eff ctrl1 ", + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); + "eff ctrl2 ", + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + "eff ctrl3 ", + actuators.control_effective[3]); } } @@ -615,7 +618,7 @@ l_optical_flow(struct listener *l) orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); + flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } static void * @@ -636,6 +639,7 @@ uorb_receive_thread(void *arg) * Might want to invoke each listener once to set initial state. */ struct pollfd fds[n_listeners]; + for (unsigned i = 0; i < n_listeners; i++) { fds[i].fd = *listeners[i].subp; fds[i].events = POLLIN; @@ -651,8 +655,10 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + } else { for (unsigned i = 0; i < n_listeners; i++) { @@ -739,7 +745,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 16759237e9e4..3832ebe708bf 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -379,6 +379,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (counter % 100 == 0) printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); } + // else // { // if(counter % 100 == 0) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ce1e52c1ba59..d94c0a69c901 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* + /* * Do not rate-limit the loop to prevent aliasing * if rate-limiting would be desired later, the line below would * enable it. @@ -125,9 +125,9 @@ mc_thread_main(int argc, char *argv[]) * orb_set_interval(att_sub, 5); */ struct pollfd fds[2] = { - { .fd = att_sub, .events = POLLIN }, - { .fd = param_sub, .events = POLLIN } - }; + { .fd = att_sub, .events = POLLIN }, + { .fd = param_sub, .events = POLLIN } + }; /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { @@ -171,6 +171,7 @@ mc_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); + } else if (ret == 0) { /* no return value, ignore */ } else { @@ -193,9 +194,11 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; orb_check(state_sub, &updated); + if (updated) { 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 */ @@ -204,9 +207,11 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); /* get a local copy of rates setpoint */ orb_check(setpoint_sub, &updated); + if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -222,6 +227,7 @@ mc_thread_main(int argc, char *argv[]) // printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; @@ -232,38 +238,42 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - /* decide wether we want rate or position input */ - } - else if (state.flag_control_manual_enabled) { - /* manual inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + } else if (state.flag_control_manual_enabled) { if (state.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(state.rc_signal_lost) { + if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - att_sp.thrust = failsafe_throttle; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; + } /* keep current yaw, do not attempt to go to north orientation, * since if the pilot regains RC control, he will be lost regarding @@ -285,50 +295,78 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } - /* only move setpoint if manual input is != 0 */ + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; + } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; } - control_yaw_position = true; } - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } - } - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ @@ -339,6 +377,7 @@ mc_thread_main(int argc, char *argv[]) /* get current rate setpoint */ bool rates_sp_valid = false; orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); } @@ -366,6 +405,7 @@ mc_thread_main(int argc, char *argv[]) /* 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); @@ -387,6 +427,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: multirotor_att_control [-m ] [-t] {start|status|stop}\n"); fprintf(stderr, " is 'rates' or 'attitude'\n"); fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); @@ -404,22 +445,25 @@ int multirotor_att_control_main(int argc, char *argv[]) motor_test_mode = true; optioncount += 1; break; + case ':': usage("missing parameter"); break; + default: fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); break; } } + argc -= optioncount; //argv += optioncount; if (argc < 1) usage("missing command"); - if (!strcmp(argv[1+optioncount], "start")) { + if (!strcmp(argv[1 + optioncount], "start")) { thread_should_exit = false; mc_task = task_spawn("multirotor_att_control", @@ -431,7 +475,7 @@ int multirotor_att_control_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1+optioncount], "stop")) { + if (!strcmp(argv[1 + optioncount], "stop")) { thread_should_exit = true; exit(0); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d8df..76dbb36d3476 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -158,16 +158,18 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) { static uint64_t last_run = 0; static uint64_t last_input = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); + if (last_input != att_sp->timestamp) { last_input = att_sp->timestamp; } + static int sensor_delay; sensor_delay = hrt_absolute_time() - att->timestamp; @@ -189,15 +191,15 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + 1000.0f, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,17 +208,24 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } + /* reset integral if on ground */ + if (att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT); - if(control_yaw_position) { + if (control_yaw_position) { /* control yaw rate */ /* positive error: rotate to right, negative error, rotate to left (NED frame) */ @@ -226,12 +235,14 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (yaw_error > M_PI_F) { yaw_error -= M_TWOPI_F; + } else if (yaw_error < -M_PI_F) { yaw_error += M_TWOPI_F; } rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); } + rates_sp->thrust = att_sp->thrust; motor_skip_counter++; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index abc94d617082..2cf83e443848 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -52,6 +52,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 93f7085aec85..deba1ac03723 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -148,7 +148,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators) { static float roll_control_last = 0; static float pitch_control_last = 0; @@ -157,6 +157,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static uint64_t last_input = 0; float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; + if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; } @@ -186,12 +187,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } /* calculate current control outputs */ - + /* control pitch (forward) output */ float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; + } else { pitch_control = 0.0f; warnx("rej. NaN ctrl pitch"); @@ -199,9 +202,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control roll (left/right) output */ float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); + /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; + } else { roll_control = 0.0f; warnx("rej. NaN ctrl roll"); @@ -209,6 +214,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + /* increase resilience to faulty control inputs */ if (!isfinite(yaw_rate_control)) { yaw_rate_control = 0.0f; diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 3c3c50801491..03dec317a32c 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -51,6 +51,6 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index 62ff0b1f19dc..e06b269dc0de 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -87,12 +87,16 @@ adc_init(void) #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); + if (rCR2 & ADC_CR2_RSTCAL) return -1; + rCR2 |= ADC_CR2_CAL; up_udelay(100); + if (rCR2 & ADC_CR2_CAL) return -1; + #endif /* arbitrarily configure all channels for 55 cycle sample time */ @@ -103,7 +107,7 @@ adc_init(void) rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = + rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | @@ -118,7 +122,7 @@ adc_init(void) /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -146,6 +150,7 @@ adc_measure(unsigned channel) /* wait for the conversion to complete */ hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ @@ -159,5 +164,5 @@ adc_measure(unsigned channel) uint16_t result = rDR; perf_end(adc_perf); - return result; -} \ No newline at end of file + return result; +} \ No newline at end of file diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 65d199fe5607..e51c2771afdc 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include #include +#include #include #include @@ -139,8 +140,9 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) { report.rc_channel[i] = system_state.rc_channel_data[i]; + } report.channel_count = system_state.rc_channels; report.armed = system_state.armed; @@ -188,7 +190,7 @@ comms_main(void) system_state.adc_in5 = adc_measure(ADC_IN5); - system_state.overcurrent = + system_state.overcurrent = (OVERCURRENT_SERVO ? (1 << 0) : 0) | (OVERCURRENT_ACC ? (1 << 1) : 0); @@ -205,7 +207,19 @@ comms_handle_config(const void *buffer, size_t length) if (length != sizeof(*cfg)) return; - /* XXX */ + /* fetch the rc mappings */ + for (unsigned i = 0; i < 4; i++) { + system_state.rc_map[i] = cfg->rc_map[i]; + } + + /* fetch the rc channel attributes */ + for (unsigned i = 0; i < 4; i++) { + system_state.rc_min[i] = cfg->rc_min[i]; + system_state.rc_trim[i] = cfg->rc_trim[i]; + system_state.rc_max[i] = cfg->rc_max[i]; + system_state.rc_rev[i] = cfg->rc_rev[i]; + system_state.rc_dz[i] = cfg->rc_dz[i]; + } } static void @@ -215,7 +229,7 @@ comms_handle_command(const void *buffer, size_t length) if (length != sizeof(*cmd)) return; - + irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -227,31 +241,59 @@ comms_handle_command(const void *buffer, size_t length) system_state.armed = false; system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; - system_state.fmu_data_received = true; + system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; + system_state.manual_override_ok = cmd->manual_override_ok; + system_state.mixer_fmu_available = true; + system_state.fmu_data_received_time = hrt_absolute_time(); + + /* set PWM update rate if changed (after limiting) */ + uint16_t new_servo_rate = cmd->servo_rate; + + /* reject faster than 500 Hz updates */ + if (new_servo_rate > 500) { + new_servo_rate = 500; + } - /* handle changes signalled by FMU */ -// if (!system_state.arm_ok && system_state.armed) -// system_state.armed = false; + /* reject slower than 50 Hz updates */ + if (new_servo_rate < 50) { + new_servo_rate = 50; + } + + if (system_state.servo_rate != new_servo_rate) { + up_pwm_servo_set_rate(new_servo_rate); + system_state.servo_rate = new_servo_rate; + } + + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ + mixer_tick(); - /* handle relay state changes here */ + /* handle relay state changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { if (system_state.relays[i] != cmd->relay_state[i]) { switch (i) { case 0: POWER_ACC1(cmd->relay_state[i]); break; + case 1: POWER_ACC2(cmd->relay_state[i]); break; + case 2: POWER_RELAY1(cmd->relay_state[i]); break; + case 3: POWER_RELAY2(cmd->relay_state[i]); break; } } + system_state.relays[i] = cmd->relay_state[i]; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 43e811ab0d9c..169d5bb81736 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -60,6 +60,10 @@ #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 + static void ppm_input(void); void @@ -88,6 +92,12 @@ controls_main(void) */ bool locked = false; + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + if (fds[0].revents & POLLIN) locked |= dsm_input(); @@ -107,22 +117,39 @@ controls_main(void) if (!locked) ppm_input(); + /* check for manual override status */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + /* force manual input override */ + system_state.mixer_manual_override = true; + + } else { + /* override not engaged, use FMU */ + system_state.mixer_manual_override = false; + } + /* * If we haven't seen any new control data in 200ms, assume we * have lost input and tell FMU. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + if (system_state.rc_channels > 0) { + /* + * If the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due = true; + } + /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; } - /* XXX do bypass mode, etc. here */ - - /* do PWM output updates */ + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ mixer_tick(); } } @@ -141,8 +168,9 @@ ppm_input(void) /* PPM data exists, copy it */ system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) + for (unsigned i = 0; i < ppm_decoded_channels; i++) { system_state.rc_channel_data[i] = ppm_buffer[i]; + } /* copy the timestamp and clear it */ system_state.rc_channels_timestamp = ppm_last_valid_decode; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index d21b3a89888a..ed7d684b6fb0 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -47,8 +47,14 @@ #include #include #include +#include + +#include #include +#include +#include + #include extern "C" { @@ -57,10 +63,16 @@ extern "C" { } /* - * Count of periodic calls in which we have no FMU input. + * Maximum interval in us before FMU signal is considered lost */ -static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 20 +#define FMU_INPUT_DROP_LIMIT_US 200000 + +/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ +#define ROLL 0 +#define PITCH 1 +#define YAW 2 +#define THROTTLE 3 +#define OVERRIDE 4 /* current servo arm/disarm state */ bool mixer_servos_armed = false; @@ -69,6 +81,8 @@ bool mixer_servos_armed = false; static uint16_t *control_values; static int control_count; +static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; + static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, @@ -81,37 +95,80 @@ mixer_tick(void) { bool should_arm; + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("RX timeout\n"); + } + /* * Decide which set of inputs we're using. */ - if (system_state.mixer_use_fmu) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - /* check that we are receiving fresh data from the FMU */ - if (!system_state.fmu_data_received) { - fmu_input_drops++; - - /* too many frames without FMU input, time to go to failsafe */ - if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_use_fmu = false; + + /* this is for planes, where manual override makes sense */ + if (system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + + } else if (system_state.rc_channels > 0) { + /* when override is on or the fmu is not available, but RC is present */ + control_count = system_state.rc_channels; + + sched_lock(); + + /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ + rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; + rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; + rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; + rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; + //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; + + /* get the remaining channels, no remapping needed */ + for (unsigned i = 4; i < system_state.rc_channels; i++) { + rc_channel_data[i] = system_state.rc_channel_data[i]; + } + + /* scale the control inputs */ + rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / + (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; + + if (rc_channel_data[THROTTLE] > 2000) { + rc_channel_data[THROTTLE] = 2000; + } + + if (rc_channel_data[THROTTLE] < 1000) { + rc_channel_data[THROTTLE] = 1000; } + + // lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", + // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), + // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); + control_values = &rc_channel_data[0]; + sched_unlock(); } else { - fmu_input_drops = 0; - system_state.fmu_data_received = false; - } + /* we have no control input (no FMU, no RC) */ - } else if (system_state.rc_channels > 0) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + // XXX builtin failsafe would activate here + control_count = 0; + } + //lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input */ - /* XXX builtin failsafe would activate here */ - control_count = 0; + /* if the fmu is available whe are good */ + if (system_state.mixer_fmu_available) { + control_count = PX4IO_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* @@ -138,14 +195,17 @@ mixer_tick(void) /* * If we are armed, update the servo output. */ - if (system_state.armed && system_state.arm_ok) + if (system_state.armed && system_state.arm_ok) { up_pwm_servo_set(i, system_state.servos[i]); + } } } /* * Decide whether the servos should be armed right now. + * A sufficient reason is armed state and either FMU or RC control inputs */ + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { @@ -171,7 +231,11 @@ mixer_callback(uintptr_t handle, return -1; /* scale from current PWM units (1000-2000) to mixer input values */ - control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) { + control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; + } else { + control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + } return 0; } @@ -229,4 +293,4 @@ mixer_handle_text(const void *buffer, size_t length) break; } -} \ No newline at end of file +} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index cb3ad6f2e76d..90236b40c926 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -54,9 +54,12 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t output_control[PX4IO_CONTROL_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; + uint16_t servo_rate; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ + bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ + bool arm_ok; /**< FMU allows full arming */ + bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ + bool manual_override_ok; /**< if true, IO performs a direct manual override */ }; /** @@ -82,7 +85,12 @@ struct px4io_config { uint16_t f2i_config_magic; #define F2I_CONFIG_MAGIC 0x6366 - /* XXX currently nothing here */ + uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */ + uint16_t rc_min[4]; /**< min value for each channel */ + uint16_t rc_trim[4]; /**< trim value for each channel */ + uint16_t rc_max[4]; /**< max value for each channel */ + int8_t rc_rev[4]; /**< rev value for each channel */ + uint16_t rc_dz[4]; /**< dz value for each channel */ }; /** diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 74362617d86c..bea9d59bced6 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -66,6 +66,8 @@ int user_start(int argc, char *argv[]) /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); + /* default to 50 Hz PWM outputs */ + system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 46a6be4a82ba..e388f65e30ba 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -70,43 +70,89 @@ struct sys_state_s { bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ + uint16_t servo_rate; /* output rate of servos in Hz */ - /* + /** + * Remote control input(s) channel mappings + */ + uint8_t rc_map[4]; + + /** + * Remote control channel attributes + */ + uint16_t rc_min[4]; + uint16_t rc_trim[4]; + uint16_t rc_max[4]; + int16_t rc_rev[4]; + uint16_t rc_dz[4]; + + /** * Data from the remote control input(s) */ unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; - /* + /** * Mixed servo outputs */ uint16_t servos[IO_SERVO_COUNT]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * If true, we are using the FMU controls, else RC input if available. */ - bool mixer_use_fmu; + bool mixer_manual_override; - /* + /** + * If true, FMU input is available. + */ + bool mixer_fmu_available; + + /** * If true, state that should be reported to FMU has been updated. */ bool fmu_report_due; - /* - * If true, new control data from the FMU has been received. + /** + * Last FMU receive time, in microseconds since system boot + */ + uint64_t fmu_data_received_time; + + /** + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * If true, the connection to FMU has been lost for more than a timeout interval + */ + bool fmu_lost; + + /** + * If true, FMU is ready for autonomous position control. Only used for LED indication + */ + bool vector_flight_mode_ok; + + /** + * If true, IO performs an on-board manual override with the RC channel values */ - bool fmu_data_received; + bool manual_override_ok; /* * Measured battery voltage in mV diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 93596ca75735..0cae29ac988a 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,20 +58,27 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +static unsigned counter = 0; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0xffff // always on -#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking -#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking -#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink +#define LED_PATTERN_SAFE 0xffff /**< always on */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ +#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ +#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ static unsigned blink_counter = 0; +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 2 static bool safety_button_pressed; @@ -101,12 +108,16 @@ safety_check_button(void *arg) */ safety_button_pressed = BUTTON_SAFETY; - if (safety_button_pressed) { - //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); - } - - /* Keep pressed for a while to arm */ + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -120,10 +131,11 @@ safety_check_button(void *arg) /* Disarm quickly */ } else if (safety_button_pressed && system_state.armed) { - if (counter < DISARM_COUNTER_THRESHOLD) { + + if (counter < ARM_COUNTER_THRESHOLD) { counter++; - } else if (counter == DISARM_COUNTER_THRESHOLD) { + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; @@ -147,6 +159,9 @@ safety_check_button(void *arg) } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; + + } else if (system_state.vector_flight_mode_ok) { + pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } /* Turn the LED on if we have a 1 at the current bit position */ @@ -173,7 +188,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_use_fmu) { + if (!system_state.mixer_fmu_available) { failsafe = !failsafe; } else { diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c89388be1e46..568ef8091c84 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -88,6 +88,7 @@ sbus_init(const char *device) last_rx_time = hrt_absolute_time(); debug("S.Bus: ready"); + } else { debug("S.Bus: open failed"); } @@ -209,7 +210,7 @@ sbus_decode(hrt_abstime frame_time) /* if the failsafe or connection lost bit is set, we consider the frame invalid */ if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ + (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ system_state.rc_channels = 0; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index cc74ae705dde..9f23ebbbab2b 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -69,58 +69,86 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); @@ -129,12 +157,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); -PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); -PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); + +PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); + +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); - +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 6b62eb9f2edb..d003401735e5 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -96,6 +96,8 @@ #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ +#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) + /** * Sensor app start / stop handling function * @@ -124,7 +126,7 @@ class Sensors int start(); private: - static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ #if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ @@ -170,7 +172,7 @@ class Sensors float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - float ex[_rc_max_chan_count]; + // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -185,11 +187,27 @@ class Sensors int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_mode_sw; + + int rc_map_manual_override_sw; + int rc_map_auto_mode_sw; + + int rc_map_manual_mode_sw; + int rc_map_sas_mode_sw; + int rc_map_rtl_sw; + int rc_map_offboard_ctrl_mode_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; float rc_scale_roll; float rc_scale_pitch; float rc_scale_yaw; + float rc_scale_flaps; float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -200,9 +218,11 @@ class Sensors param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t ex[_rc_max_chan_count]; + // param_t ex[_rc_max_chan_count]; param_t rc_type; + param_t rc_demix; + param_t gyro_offset[3]; param_t accel_offset[3]; param_t accel_scale[3]; @@ -213,11 +233,27 @@ class Sensors param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_mode_sw; + + param_t rc_map_manual_override_sw; + param_t rc_map_auto_mode_sw; + + param_t rc_map_manual_mode_sw; + param_t rc_map_sas_mode_sw; + param_t rc_map_rtl_sw; + param_t rc_map_offboard_ctrl_mode_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; param_t rc_scale_roll; param_t rc_scale_pitch; param_t rc_scale_yaw; + param_t rc_scale_flaps; param_t battery_voltage_scaling; } _parameter_handles; /**< handles for interesting parameters */ @@ -381,22 +417,38 @@ Sensors::Sensors() : sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles.dz[i] = param_find(nbuf); - /* channel exponential gain */ - sprintf(nbuf, "RC%d_EXP", i + 1); - _parameter_handles.ex[i] = param_find(nbuf); } _parameter_handles.rc_type = param_find("RC_TYPE"); + /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); + _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); + _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); + _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -453,10 +505,10 @@ Sensors::~Sensors() int Sensors::parameters_update() { - const unsigned int nchans = 8; + bool rc_valid = true; /* rc values */ - for (unsigned int i = 0; i < nchans; i++) { + for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); @@ -473,25 +525,24 @@ Sensors::parameters_update() if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { warnx("Failed getting dead zone for chan %d", i); } - if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { - warnx("Failed getting exponential gain for chan %d", i); - } _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + if (!isfinite(_parameters.scaling_factor[i]) || + _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || + _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + + /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; + rc_valid = false; } } - /* update RC function mappings */ - _rc.function[0] = _parameters.rc_map_throttle - 1; - _rc.function[1] = _parameters.rc_map_roll - 1; - _rc.function[2] = _parameters.rc_map_pitch - 1; - _rc.function[3] = _parameters.rc_map_yaw - 1; - _rc.function[4] = _parameters.rc_map_mode_sw - 1; + /* handle wrong values */ + if (!rc_valid) + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { @@ -511,8 +562,44 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { + warnx("Failed getting override sw chan index"); + } + if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { + warnx("Failed getting auto mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { + warnx("Failed getting manual mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { + warnx("Failed getting rtl sw chan index"); + } + if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { + warnx("Failed getting sas mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { + warnx("Failed getting offboard control mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { + warnx("Failed getting mode aux 1 index"); + } + if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { + warnx("Failed getting mode aux 2 index"); + } + if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { + warnx("Failed getting mode aux 3 index"); + } + if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { + warnx("Failed getting mode aux 4 index"); + } + if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { + warnx("Failed getting mode aux 5 index"); } if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { @@ -524,6 +611,31 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { warnx("Failed getting rc scaling for yaw"); } + if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { + warnx("Failed getting rc scaling for flaps"); + } + + /* update RC function mappings */ + _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[ROLL] = _parameters.rc_map_roll - 1; + _rc.function[PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; + _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + + _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; + _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; + _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; + + _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -917,8 +1029,23 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; - /* get a copy first, to prevent altering values */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* initialize to default values */ + manual_control.roll = NAN; + manual_control.pitch = NAN; + manual_control.yaw = NAN; + manual_control.throttle = NAN; + + manual_control.manual_mode_switch = NAN; + manual_control.manual_sas_switch = NAN; + manual_control.return_to_launch_switch = NAN; + manual_control.auto_offboard_input_switch = NAN; + + manual_control.flaps = NAN; + manual_control.aux1 = NAN; + manual_control.aux2 = NAN; + manual_control.aux3 = NAN; + manual_control.aux4 = NAN; + manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) @@ -966,44 +1093,99 @@ Sensors::ppm_poll() manual_control.timestamp = rc_input.timestamp; /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } - + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); /* * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, * so reverse sign. */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); /* throttle input */ manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* override switch input */ + manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); + /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + if (_rc.function[MANUAL_MODE] >= 0) { + manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + } + + if (_rc.function[SAS_MODE] >= 0) { + manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + } - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + if (_rc.function[RTL] >= 0) { + manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + } + + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* check if ready for publishing */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + } else { + /* advertise the rc topic */ + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + /* check if ready for publishing */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } } } @@ -1051,7 +1233,7 @@ Sensors::task_main() struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); - raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; @@ -1070,27 +1252,6 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - /* advertise the manual_control topic */ - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - - /* advertise the rc topic */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - } - /* wakeup source(s) */ struct pollfd fds[1]; diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 78e89f9516e6..fed97f101537 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -59,8 +59,100 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - + +void rot2quat(const float R[9], float Q[4]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + int32_t idx; + + /* conversion of rotation matrix to quaternion + * choose the largest component to begin with */ + q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; + q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; + q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; + q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; + + idx = 0; + + if (q0_2 < q1_2) { + q0_2 = q1_2; + + idx = 1; + } + + if (q0_2 < q2_2) { + q0_2 = q2_2; + idx = 2; + } + + if (q0_2 < q3_2) { + q0_2 = q3_2; + idx = 3; + } + + q0_2 = sqrtf(q0_2); + + /* solve for the remaining three components */ + if (idx == 0) { + q1_2 = q0_2; + q2_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[6] - R[2]) / 4.0F / q0_2; + q0_2 = (R[1] - R[3]) / 4.0F / q0_2; + + } else if (idx == 1) { + q2_2 = q0_2; + q1_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[6] + R[2]) / 4.0F / q0_2; + + } else if (idx == 2) { + q3_2 = q0_2; + q1_2 = (R[6] - R[2]) / 4.0F / q0_2; + q2_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[7] + R[5]) / 4.0F / q0_2; + + } else { + q1_2 = (R[1] - R[3]) / 4.0F / q0_2; + q2_2 = (R[6] + R[2]) / 4.0F / q0_2; + q3_2 = (R[7] + R[5]) / 4.0F / q0_2; + } + + /* return values */ + Q[0] = q1_2; + Q[1] = q2_2; + Q[2] = q3_2; + Q[3] = q0_2; +} + +void quat2rot(const float Q[4], float R[9]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + + memset(&R[0], 0, 9U * sizeof(float)); + + q0_2 = Q[0] * Q[0]; + q1_2 = Q[1] * Q[1]; + q2_2 = Q[2] * Q[2]; + q3_2 = Q[3] * Q[3]; + + R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; + R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); + R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); + R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); + R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; + R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); + R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); + R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); + R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; +} + float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure/(air_gas_constant * (temperature_celsius + absolute_null_kelvin)); + return static_pressure / (air_gas_constant * (temperature_celsius + absolute_null_kelvin)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index 33c21f8f95e3..4db6de7726ec 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,6 +44,8 @@ #include #include +#define CONSTANTS_ONE_G 9.80665f + __BEGIN_DECLS /** @@ -56,8 +58,31 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); +/** + * Converts a 3 x 3 rotation matrix to an unit quaternion. + * + * All orientations are expressed in NED frame. + * + * @param R rotation matrix to convert + * @param Q quaternion to write back to + */ +__EXPORT void rot2quat(const float R[9], float Q[4]); + +/** + * Converts an unit quaternion to a 3 x 3 rotation matrix. + * + * All orientations are expressed in NED frame. + * + * @param Q quaternion to convert + * @param R rotation matrix to write back to + */ +__EXPORT void quat2rot(const float Q[4], float R[9]); + /** * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius */ __EXPORT float get_air_density(float static_pressure, float temperature_celsius); diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 9aeab1dcc673..25d19f9adeb8 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -55,6 +55,8 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : Mixer(control_cb, cb_handle), diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 5fdc92c6a56e..f6e91b1bf9ba 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -217,9 +217,11 @@ unsigned MultirotorMixer::mix(float *outputs, unsigned space) { float roll = get_control(0, 0) * _roll_scale; + //lib_lowprintf("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); float pitch = get_control(0, 1) * _pitch_scale; float yaw = get_control(0, 2) * _yaw_scale; float thrust = get_control(0, 3); + //lib_lowprintf("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float max = 0.0f; float fixup_scale; @@ -276,7 +278,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (outputs[i] < _deadband) outputs[i] = _deadband; - return 0; + return _rotor_count; } void diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25034..49315cdc9f1e 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -183,3 +183,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo return pid->last_output; } + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index b51afef9ef32..64d6688677ca 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -72,6 +72,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT void pid_reset_integral(PID_t *pid); #endif /* PID_H_ */ diff --git a/apps/systemlib/scheduling_priorities.h b/apps/systemlib/scheduling_priorities.h new file mode 100644 index 000000000000..be1dbfcd8885 --- /dev/null +++ b/apps/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index aad2c4d9b4f5..40278c56c28c 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_controls_effective.h * * Actuator control topics - mixer inputs. * diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560afb9e..bbe429073102 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 1368cb716feb..261a8a4ad3df 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -48,29 +48,33 @@ * @{ */ -enum MANUAL_CONTROL_MODE -{ - MANUAL_CONTROL_MODE_DIRECT = 0, - MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, - MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, - MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - struct manual_control_setpoint_s { uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ + + float manual_override_switch; /**< manual override mode (mandatory) */ + float auto_mode_switch; /**< auto mode switch (mandatory) */ + + /** + * Any of the channels below may not be available and be set to NaN + * to indicate that it does not contain valid data. + */ + float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ + float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ + float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ + float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float override_mode_switch; + float flaps; /**< flap position */ - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ }; /**< manual control inputs */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index fef6ef2b3340..9dd54df915e8 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,6 +50,13 @@ * @{ */ +/** + * The number of RC channel inputs supported. + * Current (Q1/2013) radios support up to 18 channels, + * leaving at a sane value of 14. + */ +#define RC_CHANNELS_MAX 14 + /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -62,14 +69,18 @@ enum RC_CHANNELS_FUNCTION PITCH = 2, YAW = 3, OVERRIDE = 4, - FUNC_0 = 5, - FUNC_1 = 6, - FUNC_2 = 7, - FUNC_3 = 8, - FUNC_4 = 9, - FUNC_5 = 10, - FUNC_6 = 11, - RC_CHANNELS_FUNCTION_MAX = 12 + AUTO_MODE = 5, + MANUAL_MODE = 6, + SAS_MODE = 7, + RTL = 8, + OFFBOARD_MODE = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; struct rc_channels_s { @@ -78,14 +89,13 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_FUNCTION_MAX]; - uint8_t chan_count; /**< maximum number of valid channels */ + } chan[RC_CHANNELS_MAX]; + uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - uint8_t function[RC_CHANNELS_FUNCTION_MAX]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ - bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index 3e220965d9e0..fac571659d74 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,20 +50,77 @@ * @{ */ -enum PX4_CMD { - PX4_CMD_CONTROLLER_SELECTION = 1000, +/** + * Commands for commander app. + * + * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, + * but can contain additional ones. + */ +enum VEHICLE_CMD +{ + VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_ENUM_END=401, /* | */ }; +/** + * Commands for commander app. + * + * Should contain all of MAVLink's VEHICLE_CMD_RESULT values + * but can contain additional ones. + */ +enum VEHICLE_CMD_RESULT +{ + VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +}; + + struct vehicle_command_s { - float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ - float param2; /**< Parameter 2, as defined by MAVLink MAV_CMD enum. */ - float param3; /**< Parameter 3, as defined by MAVLink MAV_CMD enum. */ - float param4; /**< Parameter 4, as defined by MAVLink MAV_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink MAV_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink MAV_CMD enum. */ - float param7; /**< Parameter 7, as defined by MAVLink MAV_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by MAV_CMD enum. */ + float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ + float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ + float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ + float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ + float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ + uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 75c9ea0fc4db..06b4c5ca5cc5 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, VEHICLE_MODE_FLAG_TEST_ENABLED = 2, @@ -87,16 +87,48 @@ enum VEHICLE_MODE_FLAG { }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */ - VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */ - VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ + VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ }; -enum VEHICLE_ATTITUDE_MODE { - VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */ - VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */ +enum VEHICLE_MANUAL_CONTROL_MODE { + VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ + VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +}; + +enum VEHICLE_MANUAL_SAS_MODE { + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ + VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ + VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +}; + +/** + * Should match 1:1 MAVLink's MAV_TYPE ENUM + */ +enum VEHICLE_TYPE { + VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET=9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + VEHICLE_TYPE_KITE=17, /* Kite | */ + VEHICLE_TYPE_ENUM_END=18, /* | */ }; enum VEHICLE_BATTERY_WARNING { @@ -122,10 +154,9 @@ struct vehicle_status_s commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - - // uint8_t mode; - + enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ @@ -172,9 +203,12 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; -// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ - bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - + bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool flag_local_position_valid; + bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_valid_launch_position; /**< indicates a valid launch position */ }; /**