The unified diff between revisions [d0420ebd..] and [9142f333..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'dcm.c'
# # old_revision [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7] # # patch "dcm.c" # from [34486e2e7948509c6f2648d365feb84c22ab5b14] # to [441585448f6476586c2fbcb260b7a6d908cab479] # ============================================================ --- dcm.c 34486e2e7948509c6f2648d365feb84c22ab5b14 +++ dcm.c 441585448f6476586c2fbcb260b7a6d908cab479 @@ -9,14 +9,28 @@ #include "dcm.h" #include "uart.h" #include "motor.h" +#include "status.h" +#include "abs.h" +#include "log.h" +#if 0 #define GRAVITY 9.80665f +#endif +#define GRAVITY 1.0f + #define KP_ROLLPITCH 0.05967 #define KI_ROLLPITCH 0.00001278 #define ERROR_LIMIT 1.17f +/* Maximum allowed error for arming */ +#define ERROR_THRESHOLD 0.20f + +#define LOG_MAGIC_DCM_UPDATE 0x00DC111A +#define LOG_MAGIC_DCM_DRIFT 0x00DC111B + + /* Implementation of the DCM IMU concept as described by Premerlani * and Bizard */ @@ -30,8 +44,16 @@ float omega_x, omega_y, omega_z; float omega_x, omega_y, omega_z; -float delta_t = 0.01; +float delta_t = 0.005; +void dcm_log(unsigned int magic) +{ + int i; + log_put_uint(magic); + for (i = 0; i < 9; i++) + log_put_float(dcm[i]); +} + void dcm_update(float x, float y, float z) { omega_x = x + omega_i[0] + omega_p[0]; @@ -51,6 +73,8 @@ void dcm_update(float x, float y, float matrix_add(dcm, dcm, temp_matrix, 3, 3); dcm_normalise(); + +/* dcm_log(LOG_MAGIC_DCM_UPDATE); */ } void dcm_setvector(float x, float y, float z) @@ -173,6 +197,15 @@ void dcm_drift_correction(float x, float error[1] = dcm[8]*x - dcm[6]*z; error[2] = dcm[6]*y - dcm[7]*x; + if (!status_armed()) { + if ((abs(error[0]) < ERROR_THRESHOLD) && + (abs(error[1]) < ERROR_THRESHOLD) && + (abs(error[2]) < ERROR_THRESHOLD)) + status_set_ready(STATUS_MODULE_DCM_ERROR, TRUE); + else + status_set_ready(STATUS_MODULE_DCM_ERROR, FALSE); + } + for (i = 0; i < 3; i++) { if (error[i] > ERROR_LIMIT) error[i] = ERROR_LIMIT; @@ -185,6 +218,8 @@ void dcm_drift_correction(float x, float omega_i[i] += error[i] * (KI_ROLLPITCH * weight); } + dcm_log(LOG_MAGIC_DCM_DRIFT); + #if 0 putstr("w: "); putint_s((int)(weight * 100000.0f)); @@ -208,6 +243,9 @@ void dcm_drift_correction(float x, float #endif } +/* Maximum angle to the horizontal for arming: 30 degrees */ +#define ATTITUDE_THRESHOLD (0.5) + void dcm_attitude_error(float roll, float pitch, float yaw) { /* dcm[6] = sine of pitch */ @@ -221,6 +259,14 @@ void dcm_attitude_error(float roll, floa /* TODO: What if we are upside down? */ + if (!status_armed()) { + if ((abs(dcm[6]) < ATTITUDE_THRESHOLD) && + (abs(dcm[7]) < ATTITUDE_THRESHOLD)) + status_set_ready(STATUS_MODULE_ATTITUDE, TRUE); + else + status_set_ready(STATUS_MODULE_ATTITUDE, FALSE); + } + motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z); }