diff options
-rw-r--r-- | apps/commander/commander.c | 54 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 30 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 52 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 11 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 12 |
5 files changed, 105 insertions, 54 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2a1ed7532..3236c2313 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -103,8 +103,8 @@ static int stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; -static void do_gyro_calibration(void); -static void do_mag_calibration(void); +static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status); +static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); /* pthread loops */ @@ -229,13 +229,17 @@ void cal_bsort(int16_t a[], int n) } } -void do_mag_calibration(void) +void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) { + /* set to mag calibration mode */ + current_status->preflight_mag_calibration = true; + state_machine_publish(status_pub, current_status); + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; /* 30 seconds */ - const uint64_t calibration_interval_us = 5 * 1000000; + const uint64_t calibration_interval_us = 10 * 1000000; unsigned int calibration_counter = 0; const int peak_samples = 2000; @@ -264,8 +268,7 @@ void do_mag_calibration(void) } uint64_t calibration_start = hrt_absolute_time(); - while ((hrt_absolute_time() - calibration_start) < calibration_interval_us - && calibration_counter < peak_samples) { + while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; @@ -305,6 +308,10 @@ void do_mag_calibration(void) } } + /* disable calibration mode */ + current_status->preflight_mag_calibration = false; + state_machine_publish(status_pub, current_status); + /* sort values */ cal_bsort(mag_minima[0], peak_samples); cal_bsort(mag_minima[1], peak_samples); @@ -389,7 +396,7 @@ void do_mag_calibration(void) close(sub_sensor_combined); } -void do_gyro_calibration(void) +void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status) { const int calibration_count = 3000; @@ -446,8 +453,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { - - case MAV_CMD_DO_SET_MODE: { update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1); @@ -509,17 +514,36 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); - do_gyro_calibration(); - result = MAV_RESULT_ACCEPTED; + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); + do_gyro_calibration(status_pub, ¤t_status); + do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + result = MAV_RESULT_ACCEPTED; + } else { + mavlink_log_info(mavlink_fd, "[commander] REJECTING gyro calibration"); + result = MAV_RESULT_DENIED; + } handled = true; } /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); - do_mag_calibration(); - result = MAV_RESULT_ACCEPTED; + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); + do_mag_calibration(status_pub, ¤t_status); + do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + result = MAV_RESULT_ACCEPTED; + } else { + mavlink_log_info(mavlink_fd, "[commander] REJECTING mag calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; } /* none found */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2171ccedf..91eea2117 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -4,7 +4,6 @@ * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * - * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: @@ -34,7 +33,10 @@ * ****************************************************************************/ -/* @file State machine helper functions implementations */ +/** + * @file state_machine_helper.c + * State machine helper functions implementations + */ #include <stdio.h> #include "state_machine_helper.h" @@ -64,6 +66,8 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co { int invalid_state = false; + commander_state_machine_t old_state = current_status->state_machine; + switch (new_state) { case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ @@ -103,6 +107,12 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co case SYSTEM_STATE_PREFLIGHT: //global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO); + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + invalid_state = false; + } else { + invalid_state = true; + } break; case SYSTEM_STATE_REBOOT: @@ -143,16 +153,20 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co break; } - if (invalid_state == false) { - //publish the new state + if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[new_state]); + state_machine_publish(status_pub, current_status); } } +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) { + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); +} + /* * Private functions, update the state machine diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 0abc6349c..540182e4f 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -4,7 +4,6 @@ * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * - * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: @@ -34,7 +33,10 @@ * ****************************************************************************/ -/* @file State machine helper functions definitions */ +/** + * @file state_machine_helper.h + * State machine helper functions definitions + */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ @@ -47,14 +49,13 @@ #include <v1.0/common/mavlink.h> -/* - * @brief switch to new state with no checking * +/** + * Switch to new state with no checking. * * do_state_update: this is the functions that all other functions have to call in order to update the state. * the function does not question the state change, this must be done before * The function performs actions that are connected with the new state (buzzer, reboot, ...) * - * */ void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state); @@ -69,42 +70,45 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co // void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -/* - * @brief handle state machine if got position fix +/** + * Handle state machine if got position fix */ void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if position fix lost +/** + * Handle state machine if position fix lost */ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if user wants to arm +/** + * Handle state machine if user wants to arm */ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if user wants to disarm +/** + * Handle state machine if user wants to disarm */ void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if mode switch is manual +/** + * Handle state machine if mode switch is manual */ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if mode switch is stabilized +/** + * Handle state machine if mode switch is stabilized */ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status); -/* - * @brief handle state machine if mode switch is auto +/** + * Handle state machine if mode switch is auto */ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status); - +/** + * Publish current state information + */ +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status); /* @@ -113,13 +117,13 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur * */ -/* - * @brief handles *incoming request* to switch to a specific state, if state change is successful returns 0 +/** + * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 */ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode); -/* - * @brief handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 +/** + * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 */ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e3c8e5fa1..764c9ffed 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -269,8 +269,13 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t //TODO: Make this correct switch (c_status->state_machine) { case SYSTEM_STATE_PREFLIGHT: - *mavlink_state = MAV_STATE_UNINIT; - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) { + *mavlink_state = MAV_STATE_CALIBRATING; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } else { + *mavlink_state = MAV_STATE_UNINIT; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } break; case SYSTEM_STATE_STANDBY: diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 7e8eac453..45e7687fd 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -316,6 +316,8 @@ int sensors_main(int argc, char *argv[]) int16_t acc_offset[3] = {0, 0, 0}; int16_t gyro_offset[3] = {0, 0, 0}; + bool mag_calibration_enabled = false; + #pragma pack(push,1) struct adc_msg4_s { uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ @@ -345,9 +347,7 @@ int sensors_main(int argc, char *argv[]) #endif /* initialize to 100 to execute immediately */ int paramcounter = 100; - int excessive_readout_time_counter = 0; - int read_loop_counter = 0; /* Empty sensor buffers, avoid junk values */ @@ -565,10 +565,14 @@ int sensors_main(int argc, char *argv[]) if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); /* start calibration mode if requested */ - if (raw.magnetometer_mode == MAGNETOMETER_MODE_NORMAL && vstatus.preflight_mag_calibration) { + if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); - } else if (raw.magnetometer_mode != MAGNETOMETER_MODE_NORMAL && !vstatus.preflight_mag_calibration) { + printf("[sensors] enabling mag calibration mode\n"); + mag_calibration_enabled = true; + } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); + printf("[sensors] disabling mag calibration mode\n"); + mag_calibration_enabled = false; } ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); |