fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 07:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into airspeed_test_fix

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-11 22:03:05 +02:00
parent 25f3f6e7f2 66e840ebd7
commit 21ce6676a1
10 changed files with 142 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+16 -8
View File
@@ -286,15 +286,22 @@ int commander_main(int argc, char *argv[])
exit(0);
}
/* commands needing the app to run below */
if (!thread_running) {
warnx("\tcommander not started");
exit(1);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\tcommander is running");
print_status();
} else {
warnx("\tcommander not started");
}
print_status();
exit(0);
}
if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
int checkres = prearm_check(&status, mavlink_fd_local);
close(mavlink_fd_local);
warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
exit(0);
}
@@ -303,7 +310,7 @@ int commander_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "2")) {
if (!strcmp(argv[1], "disarm")) {
arm_disarm(false, mavlink_fd, "command line");
exit(0);
}
@@ -324,6 +331,7 @@ void usage(const char *reason)
void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
/* read all relevant states */
src/modules/commander/state_machine_helper.cpp
+14 -18
View File
@@ -71,8 +71,6 @@
#endif
static const int ERROR = -1;
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -623,12 +621,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
bool failed = false;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
ret = fd;
failed = true;
goto system_eval;
}
@@ -636,6 +635,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
failed = true;
goto system_eval;
}
@@ -645,29 +645,25 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_scale = 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_scale < 9.78f || accel_scale > 9.83f) {
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
}
if (accel_scale > 30.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_info(mavlink_fd, "#audio: hold still while arming");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
} else {
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
}
if (!status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
struct airspeed_s airspeed;
@@ -675,17 +671,17 @@ 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");
failed = true;
goto system_eval;
}
if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WIND OR CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
}
system_eval:
close(fd);
return ret;
return (failed);
}
src/modules/commander/state_machine_helper.h
+2
View File
@@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/mavlink/mavlink_messages.cpp
+23 -32
View File
@@ -1051,10 +1051,16 @@ protected:
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
/* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
/* set number of valid outputs depending on vehicle type */
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (mavlink_system.type) {
@@ -1071,59 +1077,44 @@ protected:
break;
}
/* scale / assign outputs depending on system type */
float out[8];
for (unsigned i = 0; i < 8; i++) {
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
out[i] = 0.0f;
/* scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
}
} else {
out[i] = -1.0f;
/* send 0 when disarmed */
out[i] = 0.0f;
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
} else {
/* fixed wing: scale all channels except throttle -1 .. 1
* because we know that we set the mixers up this way
*/
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
/* scale PWM out 900..2100 us to -1..1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
/* scale PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
}
};