fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into dataman_state_nav_rewrite

Browse Source
This commit is contained in:
Lorenz Meier 2014-07-11 16:59:01 +02:00
commit 48a614c2cb
8 changed files with 107 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Tools/check_submodules.sh
View File

@ -2,12 +2,19 @@
if [ -d NuttX/nuttx ];
then
STATUSRETVAL=$(git status --porcelain | grep -i "NuttX")
STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked NuttX submodule, correct version found"
else
echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'"
echo ""
echo ""
echo "NuttX sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else
@ -18,12 +25,19 @@ fi
if [ -d mavlink/include/mavlink/v1.0 ];
then
STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0")
STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked mavlink submodule, correct version found"
else
echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'"
echo ""
echo ""
echo "mavlink sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else

16
src/drivers/drv_px4flow.h
View File

@ -46,10 +46,18 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* @addtogroup topics
* @{
*/
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*
* @warning If possible the usage of the raw flow and performing rotation-compensation
* using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
@ -57,14 +65,18 @@ struct px4flow_report {
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/**
* @}
*/
/*
* ObjDev tag for px4flow data.
*/

9
src/drivers/drv_range_finder.h
View File

@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE {
RANGE_FINDER_TYPE_LASER = 0,
};
/**
* @addtogroup topics
* @{
*/
/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
@ -64,6 +69,10 @@ struct range_finder_report {
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
};
/**
* @}
*/
/*
* ObjDev tag for raw range finder data.
*/

9
src/drivers/drv_rc_input.h
View File

@ -67,6 +67,11 @@
*/
#define RC_INPUT_RSSI_MAX 255
/**
* @addtogroup topics
* @{
*/
/**
* Input signal type, value is a control position from zero to 100
* percent.
@ -141,6 +146,10 @@ struct rc_input_values {
rc_input_t values[RC_INPUT_MAX_CHANNELS];
};
/**
* @}
*/
/*
* ObjDev tag for R/C inputs.
*/

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

@ -288,15 +288,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);
}
@ -305,7 +312,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);
}
@ -326,6 +333,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 */

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

@ -70,8 +70,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
@ -622,12 +620,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;
}
@ -635,6 +634,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;
}
@ -644,33 +644,31 @@ 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 = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
if (fd <= 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
ret = fd;
failed = true;
goto system_eval;
}
@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
}
}
system_eval:
close(fd);
return ret;
return (failed);
}

2
src/modules/commander/state_machine_helper.h
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_ */

55
src/modules/mavlink/mavlink_messages.cpp
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);
}
}
};