diff options
-rw-r--r-- | apps/commander/calibration_routines.c | 339 | ||||
-rw-r--r-- | apps/commander/calibration_routines.h | 2 | ||||
-rw-r--r-- | apps/commander/commander.c | 195 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 50 |
4 files changed, 346 insertions, 240 deletions
diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c index dbf6cacaf..a26938637 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 063823109..e3e7fbafd 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 c39c3d6db..ea2876c7b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -247,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); } @@ -267,11 +268,13 @@ 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) { +void tune_error(void) +{ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -298,9 +301,11 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) /* store to permanent storage */ /* auto-save to EEPROM */ int save_ret = param_save_default(); - if(save_ret != 0) { + + if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + mavlink_log_info(mavlink_fd, "[cmd] trim calibration done"); } @@ -329,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}; @@ -344,6 +349,7 @@ 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, "[cmd] failed to set scale / offsets for mag"); @@ -366,9 +372,9 @@ 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"); @@ -382,14 +388,14 @@ 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++; @@ -397,7 +403,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) sprintf(buf, "[cmd] Please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); tune_confirm(); - + axis_deadline += calibration_interval / 3; } @@ -444,6 +450,7 @@ 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, "[cmd] mag cal canceled"); @@ -477,6 +484,7 @@ 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 */ @@ -507,15 +515,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* 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"); } 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); - + (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); @@ -560,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, @@ -568,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) { @@ -583,6 +594,7 @@ 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, "[cmd] gyro calibration aborted, retry"); @@ -603,7 +615,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting X gyro offset failed!"); } - + if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting Y gyro offset failed!"); } @@ -614,7 +626,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) /* 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], @@ -622,13 +634,16 @@ 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"); } @@ -642,6 +657,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) tune_confirm(); sleep(2); /* third beep by cal end routine */ + } else { mavlink_log_info(mavlink_fd, "[cmd] gyro calibration FAILED (NaN)"); } @@ -675,9 +691,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 */ @@ -689,20 +708,22 @@ 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, "[cmd] 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; @@ -712,7 +733,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } - + if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } @@ -724,7 +745,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } - + if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } @@ -742,13 +763,16 @@ 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"); } @@ -762,6 +786,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) tune_confirm(); sleep(2); /* third beep by cal end routine */ + } else { mavlink_log_info(mavlink_fd, "[cmd] accel calibration FAILED (NaN)"); } @@ -788,28 +813,32 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_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 = VEHICLE_CMD_RESULT_ACCEPTED; + } else { result = VEHICLE_CMD_RESULT_DENIED; } } break; - case VEHICLE_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 = VEHICLE_CMD_RESULT_ACCEPTED; + } else { 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 = VEHICLE_CMD_RESULT_ACCEPTED; + } else { result = VEHICLE_CMD_RESULT_DENIED; } @@ -818,11 +847,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; /* request for an autopilot reboot */ - case VEHICLE_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 = VEHICLE_CMD_RESULT_ACCEPTED; + } else { /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; @@ -855,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // break; // /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { bool handled = false; /* gyro calibration */ @@ -871,10 +901,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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, "[cmd] REJECTING gyro calibration"); result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -891,10 +923,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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, "[cmd] REJECTING mag calibration"); result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -906,10 +940,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[cmd] zero altitude cal. not implemented"); tune_confirm(); + } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING altitude calibration"); result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -926,10 +962,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta 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, "[cmd] REJECTING trim calibration"); result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -946,10 +984,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration"); result = VEHICLE_CMD_RESULT_DENIED; } + handled = true; } @@ -962,14 +1002,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - case VEHICLE_CMD_PREFLIGHT_STORAGE: { + 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))) { + ((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, "[cmd] REJECTING save cmd while multicopter armed"); + } else { // XXX move this to LOW PRIO THREAD of commander app @@ -979,23 +1020,28 @@ 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"); mavlink_log_info(mavlink_fd, param_get_default_file()); 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, param_get_default_file()); 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, param_get_default_file()); + } else { mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); mavlink_log_info(mavlink_fd, param_get_default_file()); } + result = VEHICLE_CMD_RESULT_FAILED; } @@ -1003,6 +1049,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* 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, param_get_default_file()); @@ -1012,10 +1059,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (write_ret < -1) { mavlink_log_info(mavlink_fd, "[cmd] 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, param_get_default_file()); } + result = VEHICLE_CMD_RESULT_FAILED; } @@ -1027,7 +1076,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - default: { + default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command rejection */ @@ -1038,9 +1087,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { ioctl(buzzer, TONE_SET_ALARM, 5); + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_confirm(); } @@ -1062,7 +1112,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 } }; @@ -1078,6 +1128,7 @@ static void *orb_receive_loop(void *arg) //handles status information coming fr /* mark / unmark as present */ if (info.present) { vstatus->onboard_control_sensors_present |= info.subsystem_type; + } else { vstatus->onboard_control_sensors_present &= ~info.subsystem_type; } @@ -1085,6 +1136,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; } @@ -1092,6 +1144,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; } @@ -1142,6 +1195,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)); @@ -1157,6 +1211,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -1165,7 +1220,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(). */ @@ -1201,9 +1256,11 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tcommander is running\n"); + } else { printf("\tcommander not started\n"); } + exit(0); } @@ -1360,23 +1417,28 @@ int commander_thread_main(int argc, char *argv[]) /* 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_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); @@ -1384,8 +1446,10 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ 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 */ @@ -1396,14 +1460,17 @@ int commander_thread_main(int argc, char *argv[]) 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 { printf("ARMED, rejecting sys type change\n"); } @@ -1411,6 +1478,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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); @@ -1419,6 +1487,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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); @@ -1426,6 +1495,7 @@ int commander_thread_main(int argc, char *argv[]) } orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); battery_voltage = battery.voltage_v; @@ -1507,6 +1577,7 @@ int commander_thread_main(int argc, char *argv[]) /* write to sys_status */ if (battery_voltage_valid) { current_status.voltage_battery = battery_voltage; + } else { current_status.voltage_battery = 0.0f; } @@ -1560,13 +1631,15 @@ int commander_thread_main(int argc, char *argv[]) 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; } @@ -1576,8 +1649,9 @@ int commander_thread_main(int argc, char *argv[]) * for vector flight mode. */ if (current_status.flag_local_position_valid || - current_status.flag_global_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; } @@ -1594,14 +1668,14 @@ int commander_thread_main(int argc, char *argv[]) * 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) { + !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 @@ -1762,11 +1836,11 @@ int commander_thread_main(int argc, char *argv[]) * 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)) { + (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; @@ -1793,6 +1867,7 @@ int commander_thread_main(int argc, char *argv[]) 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.manual_override_switch < -STICK_ON_OFF_LIMIT) { /* check auto mode switch for correct mode */ if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { @@ -1803,6 +1878,7 @@ int commander_thread_main(int argc, char *argv[]) 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); @@ -1812,6 +1888,7 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; mavlink_log_critical(mavlink_fd, "[cmd] DETECTED RC SIGNAL FIRST TIME."); + } else { if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } @@ -1822,6 +1899,7 @@ 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 */ @@ -1829,6 +1907,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "[cmd] 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; @@ -1845,7 +1924,7 @@ int commander_thread_main(int argc, char *argv[]) } } - + /* End mode switch */ @@ -1859,8 +1938,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; @@ -1884,8 +1963,9 @@ 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, "[cmd] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); + } else { if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd] OK:RECOVERY OFFBOARD CONTROL"); @@ -1903,18 +1983,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, "[cmd] 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; @@ -1925,7 +2008,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; } @@ -1943,7 +2026,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); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ff4e6ec00..4c4089f06 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); @@ -120,19 +120,21 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con 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, "[cmd] Switched to PREFLIGHT state"); + } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "[cmd] 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; @@ -140,10 +142,12 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } + break; case SYSTEM_STATE_STANDBY: @@ -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, "[cmd] 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; @@ -235,7 +244,8 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat 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 */ @@ -257,7 +267,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat 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); @@ -523,13 +533,14 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c /* 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)) { + (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 */ @@ -556,8 +567,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ 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) { + 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); @@ -573,6 +585,7 @@ void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *c 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("[cmd] position guided mode\n"); int old_mode = current_status->flight_mode; @@ -581,6 +594,7 @@ void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *c 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); } @@ -592,7 +606,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur 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("[cmd] auto mode\n"); int old_mode = current_status->flight_mode; @@ -601,6 +615,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur 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); } } @@ -609,10 +624,10 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t 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) { + && !current_status->flag_hil_enabled) { if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { /* Enable HIL on request */ current_status->flag_hil_enabled = true; @@ -620,23 +635,28 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ 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) { + current_status->flag_system_armed) { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!") + } else { mavlink_log_critical(mavlink_fd, "[cmd] 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); } |