dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'release_v1.0.0'

This commit is contained in:
Lorenz Meier 2015-05-22 07:24:57 +02:00
parent 283baff956 323759bb52
commit 9cf1b4ba7a
4 changed files with 28 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_status.msg
View File

@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state
uint8 hil_state # current hil state
bool failsafe # true if system is in failsafe state
bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT.
int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum
int32 system_id # system id, inspired by MAVLink's system ID field

5
src/modules/commander/accelerometer_calibration.cpp
View File

@ -559,7 +559,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 250;
const unsigned cal_hz = 100;
const unsigned settle_time = 30;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
@ -614,6 +614,8 @@ int do_level_calibration(int mavlink_fd) {
pitch_mean /= counter;
} else {
mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
success = false;
goto out;
}
if (fabsf(roll_mean) > 0.8f ) {
@ -628,6 +630,7 @@ int do_level_calibration(int mavlink_fd) {
success = true;
}
out:
if (success) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
return 0;

10
src/modules/commander/commander.cpp
View File

@ -1611,7 +1611,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@ -2698,6 +2698,8 @@ void *commander_low_prio_loop(void *arg)
false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
} else {
status.calibration_enabled = true;
}
if ((int)(cmd.param1) == 1) {
@ -2757,12 +2759,14 @@ void *commander_low_prio_loop(void *arg)
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
calib_ret = OK;
calib_ret = OK;
}
/* this always succeeds */
calib_ret = OK;
}
status.calibration_enabled = false;
if (calib_ret == OK) {
tune_positive(true);
@ -2780,7 +2784,7 @@ void *commander_low_prio_loop(void *arg)
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
} else {
tune_negative(true);

29
src/modules/commander/state_machine_helper.cpp
View File

@ -219,23 +219,13 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
valid_transition = true;
}
// Sensors need to be initialized for STANDBY state, except for HIL
if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(!status->condition_system_sensors_initialized)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming");
} else {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
@ -243,11 +233,24 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
feedback_provided = true;
} else {
// Silent ignore
feedback_provided = true;
}
// Sensors need to be initialized for STANDBY state, except for HIL
} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
if (!fRunPreArmChecks) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
}
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
// Finish up the state transition