forked from Archive/PX4-Autopilot
Code style
This commit is contained in:
parent
d6116d9564
commit
9ab20b11b6
|
@ -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) {
|
||||
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 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 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 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 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;
|
||||
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++) {
|
||||
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];
|
||||
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];
|
||||
x_sumplain += x[i];
|
||||
x_sumsq += x2;
|
||||
x_sumcube += x2 * x[i];
|
||||
|
||||
y_sumplain += y[i];
|
||||
y_sumsq += y2;
|
||||
y_sumcube += y2 * y[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];
|
||||
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];
|
||||
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];
|
||||
x2y_sum += x2 * y[i];
|
||||
x2z_sum += x2 * z[i];
|
||||
|
||||
y2x_sum += y2 * x[i];
|
||||
y2z_sum += y2 * z[i];
|
||||
y2x_sum += y2 * x[i];
|
||||
y2z_sum += y2 * z[i];
|
||||
|
||||
z2x_sum += z2 * x[i];
|
||||
z2y_sum += z2 * y[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 )
|
||||
//
|
||||
//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] )
|
||||
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);
|
||||
//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;
|
||||
//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);
|
||||
//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;
|
||||
//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;
|
||||
//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++;
|
||||
//Iterate N times, ignore stop condition.
|
||||
int n = 0;
|
||||
|
||||
//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;
|
||||
while (n < max_iterations) {
|
||||
n++;
|
||||
|
||||
//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);
|
||||
//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;
|
||||
|
||||
//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
|
||||
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);
|
||||
|
||||
//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 );
|
||||
}
|
||||
//Check for stop condition
|
||||
dA = (nA - A);
|
||||
dB = (nB - B);
|
||||
dC = (nC - C);
|
||||
|
||||
*sphere_x = A;
|
||||
*sphere_y = B;
|
||||
*sphere_z = C;
|
||||
*sphere_radius = sqrtf(Rsq);
|
||||
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
|
||||
|
||||
return 0;
|
||||
//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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue