diff options
-rw-r--r-- | apps/commander/commander.c | 196 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 4 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 5 |
3 files changed, 152 insertions, 53 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 09da50480..88d25bfab 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -127,6 +127,7 @@ static int led_on(int led); static int led_off(int led); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -146,7 +147,7 @@ static void usage(const char *reason); * @param a The array to sort * @param n The number of entries in the array */ -static void cal_bsort(int16_t a[], int n); +static void cal_bsort(float a[], int n); static int buzzer_init() { @@ -238,7 +239,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(int16_t a[], int n) +static void cal_bsort(float a[], int n) { int i,j,t; for(i=0;i<n-1;i++) @@ -257,7 +258,7 @@ static void cal_bsort(int16_t a[], int n) void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ - status->preflight_mag_calibration = true; + status->flag_preflight_mag_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); @@ -336,7 +337,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } /* disable calibration mode */ - status->preflight_mag_calibration = false; + status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); /* sort values */ @@ -351,32 +352,32 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) float min_avg[3] = { 0.0f, 0.0f, 0.0f }; float max_avg[3] = { 0.0f, 0.0f, 0.0f }; - printf("start:\n"); - - for (int i = 0; i < 10; i++) { - printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - mag_minima[0][i], - mag_minima[1][i], - mag_minima[2][i], - mag_maxima[0][i], - mag_maxima[1][i], - mag_maxima[2][i]); - usleep(10000); - } - printf("-----\n"); - - for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - mag_minima[0][i], - mag_minima[1][i], - mag_minima[2][i], - mag_maxima[0][i], - mag_maxima[1][i], - mag_maxima[2][i]); - usleep(10000); - } - - printf("end\n"); + // printf("start:\n"); + + // for (int i = 0; i < 10; i++) { + // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", + // mag_minima[0][i], + // mag_minima[1][i], + // mag_minima[2][i], + // mag_maxima[0][i], + // mag_maxima[1][i], + // mag_maxima[2][i]); + // usleep(10000); + // } + // printf("-----\n"); + + // for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { + // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", + // mag_minima[0][i], + // mag_minima[1][i], + // mag_minima[2][i], + // mag_maxima[0][i], + // mag_maxima[1][i], + // mag_maxima[2][i]); + // usleep(10000); + // } + + // printf("end\n"); /* take average of center value group */ for (int i = 0; i < (peak_samples - outlier_margin); i++) { @@ -397,8 +398,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) max_avg[1] /= (peak_samples - outlier_margin); max_avg[2] /= (peak_samples - outlier_margin); - printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], - (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); + // printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], + // (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); float mag_offset[3]; @@ -418,16 +419,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; - if (param_set(param_find("SENSOR_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENSOR_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) { + + mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)"); + } else { + /* announce and set new offset */ + + char offset_output[50]; + sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); + mavlink_log_info(mavlink_fd, offset_output); + + if (param_set(param_find("SENSOR_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENSOR_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } - if (param_set(param_find("SENSOR_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + if (param_set(param_find("SENSOR_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + } } free(mag_maxima[0]); @@ -438,20 +450,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) free(mag_minima[1]); free(mag_minima[2]); - char offset_output[50]; - sprintf(offset_output, "[commander] mag cal: x:%8.4f y:%8.4f z:%8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - mavlink_log_info(mavlink_fd, offset_output); - close(sub_sensor_combined); } void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) { /* set to gyro calibration mode */ - status->preflight_gyro_calibration = true; + status->flag_preflight_gyro_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - const int calibration_count = 3000; + const int calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; @@ -494,7 +502,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) } /* exit to gyro calibration mode */ - status->preflight_gyro_calibration = false; + status->flag_preflight_gyro_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); char offset_output[50]; @@ -504,6 +512,75 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } +void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + usleep(5000); + mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + + /* set to accel calibration mode */ + status->flag_preflight_accel_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float accel_offset[3] = {0.0f, 0.0f, 0.0f}; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + if (poll(fds, 1, 1000)) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + accel_offset[0] += raw.accelerometer_m_s2[0]; + accel_offset[1] += raw.accelerometer_m_s2[1]; + accel_offset[2] += raw.accelerometer_m_s2[2]; + calibration_counter++; + } else { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + 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; + + /* 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]); + + accel_offset[2] = -(accel_offset[2] + total_len); + + if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + } + + if (param_set(param_find("SENSOR_ACC_YOFF"), &(accel_offset[1]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + } + + if (param_set(param_find("SENSOR_ACC_ZOFF"), &(accel_offset[2]))) { + mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + } + + /* exit to gyro calibration mode */ + status->flag_preflight_accel_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + char offset_output[50]; + sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], + (double)accel_offset[1], (double)accel_offset[2]); + mavlink_log_info(mavlink_fd, offset_output); + + close(sub_sensor_combined); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -622,6 +699,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration"); + do_accel_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = MAV_RESULT_ACCEPTED; + } else { + mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; + } + /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); @@ -1158,8 +1253,9 @@ int commander_thread_main(int argc, char *argv[]) /* If full run came back clean, transition to standby */ if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.preflight_gyro_calibration == false && - current_status.preflight_mag_calibration == false) { + 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 */ do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7fc5b6685..4aac6735c 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -377,7 +377,9 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t switch (c_status->state_machine) { case SYSTEM_STATE_PREFLIGHT: - if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) { + if (c_status->flag_preflight_gyro_calibration || + c_status->flag_preflight_mag_calibration || + c_status->flag_preflight_accel_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index c378b05f1..713a7a801 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -129,8 +129,9 @@ struct vehicle_status_s // bool control_speed_enabled; /**< true if speed (implies direction) is controlled */ // bool control_position_enabled; /**< true if position is controlled */ - bool preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool preflight_mag_calibration; /**< true if mag calibration is requested */ + bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + bool flag_preflight_accel_calibration; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ |