aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/state_machine_helper.cpp
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2014-07-13 12:46:49 +0200
committerLorenz Meier <lm@inf.ethz.ch>2014-07-13 12:46:49 +0200
commit91651bf240e0b2f53936f45d59ffae2c970eac9c (patch)
tree5ceadc4a54f50545b529d0af6480ba37e972b259 /src/modules/commander/state_machine_helper.cpp
parent8590d555b432f206e2b88893ea5198c1398aa99c (diff)
parenta32577377b8d735c77ffaaee15e2bbfa74be703f (diff)
downloadpx4-firmware-91651bf240e0b2f53936f45d59ffae2c970eac9c.tar.gz
px4-firmware-91651bf240e0b2f53936f45d59ffae2c970eac9c.tar.bz2
px4-firmware-91651bf240e0b2f53936f45d59ffae2c970eac9c.zip
Merged commander fixes into airspeed_test_fix
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r--src/modules/commander/state_machine_helper.cpp26
1 files changed, 13 insertions, 13 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e90c77af4..09ea12c38 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -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
-static const char *state_names[ARMING_STATE_MAX] = {
+static const char * const state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
@@ -161,7 +161,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if we need safety switch press
} 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;
}
@@ -172,16 +172,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if power is not good
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;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
- if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
- (status->avionics_power_rail_voltage < 4.9f)) {
+ if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
+ (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;
}
}
@@ -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);
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;
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);
if (ret != OK) {
- mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+ mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
failed = true;
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);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
- mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
- mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
+ mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
+ mavlink_log_critical(mavlink_fd, "hold still while arming");
/* this is frickin' fatal */
failed = true;
goto system_eval;
}
} else {
- mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
+ mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
/* this is frickin' fatal */
failed = true;
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) ||
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;
goto system_eval;
}
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
}
}