zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merged commander fixes into airspeed_test_fix

sbg
Lorenz Meier 11 years ago
parent
8590d555b4 a32577377b
commit
91651bf240
17 changed files with 192 additions and 212 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      src/include/mavlink/mavlink_log.h
  2. 3
      src/modules/commander/accelerometer_calibration.cpp
  3. 3
      src/modules/commander/accelerometer_calibration.h
  4. 2
      src/modules/commander/airspeed_calibration.h
  5. 2
      src/modules/commander/baro_calibration.cpp
  6. 2
      src/modules/commander/baro_calibration.h
  7. 3
      src/modules/commander/calibration_messages.h
  8. 3
      src/modules/commander/calibration_routines.cpp
  9. 3
      src/modules/commander/calibration_routines.h
  10. 182
      src/modules/commander/commander.cpp
  11. 10
      src/modules/commander/commander_helper.cpp
  12. 9
      src/modules/commander/commander_helper.h
  13. 2
      src/modules/commander/mag_calibration.h
  14. 26
      src/modules/commander/state_machine_helper.cpp
  15. 10
      src/modules/commander/state_machine_helper.h
  16. 125
      src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
  17. 16
      src/modules/ekf_att_pos_estimator/estimator_23states.cpp

3
src/include/mavlink/mavlink_log.h
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* 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
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions

3
src/modules/commander/accelerometer_calibration.cpp
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* *
* 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

3
src/modules/commander/accelerometer_calibration.h
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* *
* 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

2
src/modules/commander/airspeed_calibration.h
Unescape View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* *
* 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

2
src/modules/commander/baro_calibration.cpp
Unescape View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* *
* 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

2
src/modules/commander/baro_calibration.h
Unescape View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* *
* 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

3
src/modules/commander/calibration_messages.h
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* *
* 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

3
src/modules/commander/calibration_routines.cpp
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (c) 2012 PX4 Development Team. All rights reserved.
* 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
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions

3
src/modules/commander/calibration_routines.h
Unescape View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (c) 2012 PX4 Development Team. All rights reserved.
* 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
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions

182
src/modules/commander/commander.cpp
Unescape View File

@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
return arming_res; return arming_res;
} }
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
return false; return false;
} }
@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set HIL state */ /* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state // Transition the arming state
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status, MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */ /* ALTCTL */
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status, MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */ /* ACRO */
main_ret = main_state_transition(status, MAIN_STATE_ACRO); main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */ /* OFFBOARD */
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
} }
} else { } else {
/* use base mode */ /* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status, MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status, MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
} }
} }
} }
@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break; break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: { case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// We use an float epsilon delta to test float equality. // for logic state parameters
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
} else { } else {
bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
// Flick to inair restore first if this comes from an onboard system // Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE; status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
} }
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) { if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else { } else {
@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_OVERRIDE_GOTO: { case VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?) // TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
// Increase by 0.5f and rely on the integer cast
// implicit floor(). This is the *safest* way to
// convert from floats representing small ints to actual ints.
unsigned int mav_goto = (cmd->param1 + 0.5f);
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->nav_state = NAVIGATION_STATE_AUTO_LOITER; status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); mavlink_log_critical(mavlink_fd, "Pause mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->nav_state = NAVIGATION_STATE_AUTO_MISSION; status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1, (double)cmd->param1,
(double)cmd->param2, (double)cmd->param2,
(double)cmd->param3, (double)cmd->param3,
@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
//XXX: to enable the parachute, a param needs to be set //XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (use_current) { if (use_current) {
/* use current position */ /* use current position */
if (status->condition_global_position_valid) { if (status_local->condition_global_position_valid) {
home->lat = global_pos->lat; home->lat = global_pos->lat;
home->lon = global_pos->lon; home->lon = global_pos->lon;
home->alt = global_pos->alt; home->alt = global_pos->alt;
@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} }
/* mark home position as set */ /* mark home position as set */
status->condition_home_position_valid = true; status_local->condition_home_position_valid = true;
} }
} }
break; break;
@ -688,11 +697,11 @@ int commander_thread_main(int argc, char *argv[])
/* initialize */ /* initialize */
if (led_init() != 0) { if (led_init() != 0) {
warnx("ERROR: Failed to initialize leds"); warnx("ERROR: LED INIT FAIL");
} }
if (buzzer_init() != OK) { if (buzzer_init() != OK) {
warnx("ERROR: Failed to initialize buzzer"); warnx("ERROR: BUZZER INIT FAIL");
} }
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@ -766,8 +775,9 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq); mission.dataman_id, mission.count, mission.current_seq);
} else { } else {
warnx("reading mission state failed"); const char *missionfail = "reading mission state failed";
mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); warnx("%s", missionfail);
mavlink_log_critical(mavlink_fd, missionfail);
/* initialize mission state in dataman */ /* initialize mission state in dataman */
mission.dataman_id = 0; mission.dataman_id = 0;
@ -780,8 +790,6 @@ int commander_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
} }
mavlink_log_info(mavlink_fd, "[cmd] started");
int ret; int ret;
pthread_attr_t commander_low_prio_attr; pthread_attr_t commander_low_prio_attr;
@ -1074,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[])
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true; arming_state_changed = true;
} }
} }
@ -1178,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true; status_changed = true;
if (status.condition_landed) { if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "#audio: LANDED"); mavlink_log_critical(mavlink_fd, "LANDED MODE");
} else { } else {
mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); mavlink_log_critical(mavlink_fd, "IN AIR MODE");
} }
} }
} }
@ -1261,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[])
/* if battery voltage is getting lower, warn using buzzer, etc. */ /* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true; low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true; status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */ /* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true; critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) { if (armed.armed) {
@ -1330,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[])
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) { if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true; status.rc_signal_found_once = true;
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); mavlink_log_critical(mavlink_fd, "detected RC signal first time");
status_changed = true; status_changed = true;
} else { } else {
if (status.rc_signal_lost) { if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); mavlink_log_critical(mavlink_fd, "RC signal regained");
status_changed = true; status_changed = true;
} }
} }
@ -1376,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[])
* the system can be armed in auto if armed via the GCS. * the system can be armed in auto if armed via the GCS.
*/ */
if (status.main_state != MAIN_STATE_MANUAL) { if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else { } else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) { if (arming_ret == TRANSITION_CHANGED) {
@ -1396,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[])
if (arming_ret == TRANSITION_CHANGED) { if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) { if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); mavlink_log_info(mavlink_fd, "ARMED by RC");
} else { } else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); mavlink_log_info(mavlink_fd, "DISARMED by RC");
} }
arming_state_changed = true; arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) { } else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */ /* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); mavlink_log_critical(mavlink_fd, "arming state transition denied");
tune_negative(true); tune_negative(true);
} }
@ -1419,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[])
} else if (main_res == TRANSITION_DENIED) { } else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */ /* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); mavlink_log_critical(mavlink_fd, "main state transition denied");
} }
} else { } else {
if (!status.rc_signal_lost) { if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
status.rc_signal_lost = true; status.rc_signal_lost = true;
status_changed = true; status_changed = true;
} }
@ -1436,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[])
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */ /* handle the case where data link was regained */
if (telemetry_lost[i]) { if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false; telemetry_lost[i] = false;
} }
have_link = true; have_link = true;
} else { } else {
if (!telemetry_lost[i]) { if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); mavlink_log_critical(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true; telemetry_lost[i] = true;
} }
} }
@ -1458,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
if (!status.data_link_lost) { if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true; status.data_link_lost = true;
status_changed = true; status_changed = true;
} }
@ -1642,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
} }
void void
control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
{ {
/* driving rgbled */ /* driving rgbled */
if (changed) { if (changed) {
bool set_normal_color = false; bool set_normal_color = false;
/* set mode */ /* set mode */
if (status->arming_state == ARMING_STATE_ARMED) { if (status_local->arming_state == ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON); rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true; set_normal_color = true;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_color(RGBLED_COLOR_RED);
} else if (status->arming_state == ARMING_STATE_STANDBY) { } else if (status_local->arming_state == ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE); rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true; set_normal_color = true;
@ -1668,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
if (set_normal_color) { if (set_normal_color) {
/* set color */ /* set color */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
rgbled_set_color(RGBLED_COLOR_AMBER); rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else { } else {
if (status->condition_local_position_valid) { if (status_local->condition_local_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_color(RGBLED_COLOR_GREEN);
} else { } else {
@ -1706,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
#endif #endif
/* give system warnings on error LED, XXX maybe add memory usage warning too */ /* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) { if (status_local->load > 0.95f) {
if (leds_counter % 2 == 0) { if (leds_counter % 2 == 0) {
led_toggle(LED_AMBER); led_toggle(LED_AMBER);
} }
@ -1719,16 +1727,16 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
} }
transition_result_t transition_result_t
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
{ {
/* set main state according to RC switches */ /* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED; transition_result_t res = TRANSITION_DENIED;
/* offboard switch overrides main switch */ /* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) { if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_OFFBOARD); res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status, "OFFBOARD"); print_reject_mode(status_local, "OFFBOARD");
} else { } else {
return res; return res;
@ -1743,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
case SWITCH_POS_OFF: // MANUAL case SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == SWITCH_POS_ON) { if (sp_man->acro_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_ACRO); res = main_state_transition(status_local, MAIN_STATE_ACRO);
} else { } else {
res = main_state_transition(status, MAIN_STATE_MANUAL); res = main_state_transition(status_local, MAIN_STATE_MANUAL);
} }
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
case SWITCH_POS_MIDDLE: // ASSIST case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == SWITCH_POS_ON) { if (sp_man->posctl_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_POSCTL); res = main_state_transition(status_local, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "POSCTL"); print_reject_mode(status_local, "POSCTL");
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL); res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode break; // changed successfully or already in this mode
} }
if (sp_man->posctl_switch != SWITCH_POS_ON) { if (sp_man->posctl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTL"); print_reject_mode(status_local, "ALTCTL");
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL); res = main_state_transition(status_local, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
case SWITCH_POS_ON: // AUTO case SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == SWITCH_POS_ON) { if (sp_man->return_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_RTL); res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_RTL"); print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set // fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
} else if (sp_man->loiter_switch == SWITCH_POS_ON) { } else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_LOITER"); print_reject_mode(status_local, "AUTO_LOITER");
} else { } else {
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_MISSION"); print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set // fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@ -1822,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
} }
// fallback to POSCTL // fallback to POSCTL
res = main_state_transition(status, MAIN_STATE_POSCTL); res = main_state_transition(status_local, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL); res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL); res = main_state_transition(status_local, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
@ -2001,15 +2009,13 @@ set_control_mode()
} }
void void
print_reject_mode(struct vehicle_status_s *status, const char *msg) print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
{ {
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t; last_print_mode_reject_time = t;
char s[80]; mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
/* only buzz if armed, because else we're driving people nuts indoors /* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */ they really need to look at the leds as well. */
@ -2024,9 +2030,7 @@ print_reject_arm(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t; last_print_mode_reject_time = t;
char s[80]; mavlink_log_critical(mavlink_fd, msg);
sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative(true); tune_negative(true);
} }
} }
@ -2039,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break; break;
case VEHICLE_CMD_RESULT_DENIED: case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
tune_negative(true); tune_negative(true);
break; break;
case VEHICLE_CMD_RESULT_FAILED: case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
tune_negative(true); tune_negative(true);
break; break;
@ -2055,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break; break;
case VEHICLE_CMD_RESULT_UNSUPPORTED: case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
tune_negative(true); tune_negative(true);
break; break;

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

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* *
* 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
@ -37,6 +34,11 @@
/** /**
* @file commander_helper.cpp * @file commander_helper.cpp
* Commander helper functions implementations * Commander helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
*/ */
#include <stdio.h> #include <stdio.h>

9
src/modules/commander/commander_helper.h
Unescape View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@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
@ -36,6 +34,9 @@
/** /**
* @file commander_helper.h * @file commander_helper.h
* Commander helper functions definitions * Commander helper functions definitions
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/ */
#ifndef COMMANDER_HELPER_H_ #ifndef COMMANDER_HELPER_H_
@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage. * else use simple estimate based on voltage.
* *
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @return the estimated remaining capacity in 0..1 * @return the estimated remaining capacity in 0..1
*/ */
float battery_remaining_estimate_voltage(float voltage, float discharged); float battery_remaining_estimate_voltage(float voltage, float discharged);

2
src/modules/commander/mag_calibration.h
Unescape View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* *
* 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

26
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -88,7 +88,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
}; };
// You can index into the array with an arming_state_t in order to get it's textual representation // You can index into the array with an arming_state_t in order to get it's textual representation
static const char *state_names[ARMING_STATE_MAX] = { static const char * const state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT", "ARMING_STATE_INIT",
"ARMING_STATE_STANDBY", "ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED", "ARMING_STATE_ARMED",
@ -161,7 +161,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if we need safety switch press // Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) { } else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
valid_transition = false; valid_transition = false;
} }
@ -172,16 +172,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if power is not good // Fail transition if power is not good
if (!status->condition_power_input_valid) { if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
valid_transition = false; valid_transition = false;
} }
// Fail transition if power levels on the avionics rail // Fail transition if power levels on the avionics rail
// are measured but are insufficient // are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f)) { (status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
valid_transition = false; valid_transition = false;
} }
} }
@ -626,7 +626,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
failed = true; failed = true;
goto system_eval; goto system_eval;
} }
@ -634,7 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
ret = ioctl(fd, ACCELIOCSELFTEST, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) { if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
failed = true; failed = true;
goto system_eval; goto system_eval;
} }
@ -648,14 +648,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); mavlink_log_critical(mavlink_fd, "hold still while arming");
/* this is frickin' fatal */ /* this is frickin' fatal */
failed = true; failed = true;
goto system_eval; goto system_eval;
} }
} else { } else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
/* this is frickin' fatal */ /* this is frickin' fatal */
failed = true; failed = true;
goto system_eval; goto system_eval;
@ -670,13 +670,13 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) ||
hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
failed = true; failed = true;
goto system_eval; goto system_eval;
} }
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WIND OR CALIBRATION MISSING"); mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
// XXX do not make this fatal yet // XXX do not make this fatal yet
} }
} }

10
src/modules/commander/state_machine_helper.h
Unescape View File

@ -1,8 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@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
@ -36,14 +34,14 @@
/** /**
* @file state_machine_helper.h * @file state_machine_helper.h
* State machine helper functions definitions * State machine helper functions definitions
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/ */
#ifndef STATE_MACHINE_HELPER_H_ #ifndef STATE_MACHINE_HELPER_H_
#define STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_
#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>

125
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
Unescape View File

@ -559,61 +559,26 @@ FixedwingEstimator::check_filter_state()
int check = _ekf->CheckAndBound(&ekf_report); int check = _ekf->CheckAndBound(&ekf_report);
const char* ekfname = "att pos estimator: "; const char* const feedback[] = { 0,
"NaN in states, resetting",
switch (check) { "stale IMU data, resetting",
case 0: "got initial position lock",
/* all ok */ "excessive gyro offsets",
break; "GPS velocity divergence",
case 1: "excessive covariances",
{ "unknown condition"};
const char* str = "NaN in states, resetting";
warnx("%s", str); // Print out error condition
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); if (check) {
break; unsigned warn_index = static_cast<unsigned>(check);
} unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
case 2:
{ if (max_warn_index < warn_index) {
const char* str = "stale IMU data, resetting"; warn_index = max_warn_index;
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 3:
{
const char* str = "switching to dynamic state";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 4:
{
const char* str = "excessive gyro offsets";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 5:
{
const char* str = "GPS velocity divergence";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 6:
{
const char* str = "excessive covariances";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
} }
default: warnx("reset: %s", feedback[warn_index]);
{ mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
const char* str = "unknown reset condition";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
} }
struct estimator_status_report rep; struct estimator_status_report rep;
@ -645,6 +610,10 @@ FixedwingEstimator::check_filter_state()
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@ -1202,10 +1171,10 @@ FixedwingEstimator::task_main()
_baro_gps_offset = 0.0f; _baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else if (_ekf->statesInitialised) { } else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now // We're apparently initialized in this case now
int check = check_filter_state(); int check = check_filter_state();
if (check) { if (check) {
@ -1213,7 +1182,6 @@ FixedwingEstimator::task_main()
continue; continue;
} }
// Run the strapdown INS equations every IMU update // Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED(); _ekf->UpdateStrapdownEquationsNED();
#if 0 #if 0
@ -1281,7 +1249,11 @@ FixedwingEstimator::task_main()
// run the fusion step // run the fusion step
_ekf->FuseVelposNED(); _ekf->FuseVelposNED();
} else if (_ekf->statesInitialised) { } else if (!_gps_initialized) {
// force static mode
_ekf->staticMode = true;
// Convert GPS measurements to Pos NE, hgt and Vel NED // Convert GPS measurements to Pos NE, hgt and Vel NED
_ekf->velNED[0] = 0.0f; _ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f; _ekf->velNED[1] = 0.0f;
@ -1303,7 +1275,7 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false; _ekf->fusePosData = false;
} }
if (newHgtData && _ekf->statesInitialised) { if (newHgtData) {
// Could use a blend of GPS and baro alt data if desired // Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true; _ekf->fuseHgtData = true;
@ -1317,7 +1289,7 @@ FixedwingEstimator::task_main()
} }
// Fuse Magnetometer Measurements // Fuse Magnetometer Measurements
if (newDataMag && _ekf->statesInitialised) { if (newDataMag) {
_ekf->fuseMagData = true; _ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
@ -1331,7 +1303,7 @@ FixedwingEstimator::task_main()
} }
// Fuse Airspeed Measurements // Fuse Airspeed Measurements
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { if (newAdsData && _ekf->VtasMeas > 7.0f) {
_ekf->fuseVtasData = true; _ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed(); _ekf->FuseAirspeed();
@ -1399,7 +1371,7 @@ FixedwingEstimator::task_main()
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
/* crude land detector for fixedwing only, /* crude land detector for fixedwing only,
@ -1490,27 +1462,28 @@ FixedwingEstimator::task_main()
} }
} if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
} _wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
if (hrt_elapsed_time(&_wind.timestamp) > 99000) { /* lazily publish the wind estimate only once available */
_wind.timestamp = _global_pos.timestamp; if (_wind_pub > 0) {
_wind.windspeed_north = _ekf->states[14]; /* publish the wind estimate */
_wind.windspeed_east = _ekf->states[15]; orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */ } else {
if (_wind_pub > 0) { /* advertise and publish */
/* publish the wind estimate */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); }
}
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
} }
} }
} }
} }

16
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
Unescape View File

@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
void AttPosEKF::OnGroundCheck() void AttPosEKF::OnGroundCheck()
{ {
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) { if (staticMode) {
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
} }
@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
current_ekf_state.statesNaN = false; current_ekf_state.statesNaN = false;
current_ekf_state.velHealth = true; current_ekf_state.velHealth = true;
//current_ekf_state.posHealth = ?; current_ekf_state.posHealth = true;
//current_ekf_state.hgtHealth = ?; current_ekf_state.hgtHealth = true;
current_ekf_state.velTimeout = false; current_ekf_state.velTimeout = false;
//current_ekf_state.posTimeout = ?; current_ekf_state.posTimeout = false;
//current_ekf_state.hgtTimeout = ?; current_ekf_state.hgtTimeout = false;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
// Fill variables with valid data // Fill variables with valid data
velNED[0] = initvelNED[0]; velNED[0] = initvelNED[0];

Write Preview
Loading…
Cancel
Save