fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Completed calibration state machine, calibration state now propagating to sensor, scale calibration soon

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-06 23:43:09 +02:00
parent f88bba0cec
commit 7f2a63eb96
5 changed files with 105 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+39 -15
View File
@@ -103,8 +103,8 @@ static int stat_pub;
static uint16_t nofix_counter = 0; static uint16_t nofix_counter = 0;
static uint16_t gotfix_counter = 0; static uint16_t gotfix_counter = 0;
static void do_gyro_calibration(void); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
static void do_mag_calibration(void); 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); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
/* pthread loops */ /* 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)); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw; struct sensor_combined_s raw;
/* 30 seconds */ /* 30 seconds */
const uint64_t calibration_interval_us = 5 * 1000000; const uint64_t calibration_interval_us = 10 * 1000000;
unsigned int calibration_counter = 0; unsigned int calibration_counter = 0;
const int peak_samples = 2000; const int peak_samples = 2000;
@@ -264,8 +268,7 @@ void do_mag_calibration(void)
} }
uint64_t calibration_start = hrt_absolute_time(); uint64_t calibration_start = hrt_absolute_time();
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
&& calibration_counter < peak_samples) {
/* wait blocking for new data */ /* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; 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 */ /* sort values */
cal_bsort(mag_minima[0], peak_samples); cal_bsort(mag_minima[0], peak_samples);
cal_bsort(mag_minima[1], peak_samples); cal_bsort(mag_minima[1], peak_samples);
@@ -389,7 +396,7 @@ void do_mag_calibration(void)
close(sub_sensor_combined); 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; 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 */ /* request to set different system mode */
switch (cmd->command) { switch (cmd->command) {
case MAV_CMD_DO_SET_MODE: case MAV_CMD_DO_SET_MODE:
{ {
update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1); 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 */ /* gyro calibration */
if ((int)(cmd->param1) == 1) { if ((int)(cmd->param1) == 1) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); /* transition to calibration state */
do_gyro_calibration(); do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
result = MAV_RESULT_ACCEPTED;
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
do_gyro_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "[commander] REJECTING gyro calibration");
result = MAV_RESULT_DENIED;
}
handled = true; handled = true;
} }
/* magnetometer calibration */ /* magnetometer calibration */
if ((int)(cmd->param2) == 1) { if ((int)(cmd->param2) == 1) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); /* transition to calibration state */
do_mag_calibration(); do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
result = MAV_RESULT_ACCEPTED;
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
do_mag_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_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 */ /* none found */
apps/commander/state_machine_helper.c
+22 -8
View File
@@ -4,7 +4,6 @@
* Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * Julian Oes <joes@student.ethz.ch>
* *
*
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * 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 <stdio.h>
#include "state_machine_helper.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; int invalid_state = false;
commander_state_machine_t old_state = current_status->state_machine;
switch (new_state) { switch (new_state) {
case SYSTEM_STATE_MISSION_ABORT: { case SYSTEM_STATE_MISSION_ABORT: {
/* Indoor or outdoor */ /* 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: case SYSTEM_STATE_PREFLIGHT:
//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO); //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; break;
case SYSTEM_STATE_REBOOT: case SYSTEM_STATE_REBOOT:
@@ -143,16 +153,20 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
break; break;
} }
if (invalid_state == false) { if (invalid_state == false || old_state != new_state) {
//publish the new state
current_status->state_machine = new_state; current_status->state_machine = new_state;
current_status->counter++; state_machine_publish(status_pub, current_status);
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]);
} }
} }
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 * Private functions, update the state machine
apps/commander/state_machine_helper.h
+28 -24
View File
@@ -4,7 +4,6 @@
* Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * Julian Oes <joes@student.ethz.ch>
* *
*
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * 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_ #ifndef STATE_MACHINE_HELPER_H_
#define STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_
@@ -47,14 +49,13 @@
#include <v1.0/common/mavlink.h> #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. * 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 does not question the state change, this must be done before
* The function performs actions that are connected with the new state (buzzer, reboot, ...) * 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); 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); // 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); 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); 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); 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); 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); 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); 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); 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); 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); uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode);
apps/mavlink/mavlink.c
+8 -3
View File
@@ -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> * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * 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 //TODO: Make this correct
switch (c_status->state_machine) { switch (c_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT: case SYSTEM_STATE_PREFLIGHT:
*mavlink_state = MAV_STATE_UNINIT; if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *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; break;
case SYSTEM_STATE_STANDBY: case SYSTEM_STATE_STANDBY:
apps/sensors/sensors.c
+8 -4
View File
@@ -316,6 +316,8 @@ int sensors_main(int argc, char *argv[])
int16_t acc_offset[3] = {0, 0, 0}; int16_t acc_offset[3] = {0, 0, 0};
int16_t gyro_offset[3] = {0, 0, 0}; int16_t gyro_offset[3] = {0, 0, 0};
bool mag_calibration_enabled = false;
#pragma pack(push,1) #pragma pack(push,1)
struct adc_msg4_s { struct adc_msg4_s {
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
@@ -345,9 +347,7 @@ int sensors_main(int argc, char *argv[])
#endif #endif
/* initialize to 100 to execute immediately */ /* initialize to 100 to execute immediately */
int paramcounter = 100; int paramcounter = 100;
int excessive_readout_time_counter = 0; int excessive_readout_time_counter = 0;
int read_loop_counter = 0; int read_loop_counter = 0;
/* Empty sensor buffers, avoid junk values */ /* Empty sensor buffers, avoid junk values */
@@ -565,10 +565,14 @@ int sensors_main(int argc, char *argv[])
if (magcounter == 4) { /* 120 Hz */ if (magcounter == 4) { /* 120 Hz */
uint64_t start_mag = hrt_absolute_time(); uint64_t start_mag = hrt_absolute_time();
/* start calibration mode if requested */ /* 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); 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); 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)); ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));