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' into beta

Browse Source
This commit is contained in:
Lorenz Meier 2015-04-25 12:55:13 +02:00
commit 7b06d781d8
9 changed files with 50 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/13002_firefly6
View File

@ -8,7 +8,7 @@
sh /etc/init.d/rc.vtol_defaults
set MIXER firefly6
set PWM_OUT 123456
set PWM_OUT 12345678
set MIXER_AUX firefly6
set PWM_AUX_RATE 50

6
ROMFS/px4fmu_common/init.d/rcS
View File

@ -461,10 +461,16 @@ then
#
if ver hwcmp PX4FMU_V2
then
# XXX We need a better way for runtime eval of shell variables,
# but this works for now
if param compare SYS_COMPANION 921600
then
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
fi
if param compare SYS_COMPANION 57600
then
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
fi
fi
# UAVCAN

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = Rvis * v;
math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
z_k[6] = vn(0);
z_k[7] = vn(1);
z_k[8] = vn(2);

8
src/modules/commander/PreflightCheck.cpp
View File

@ -93,7 +93,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@ -137,7 +137,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@ -161,7 +161,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
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_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
/* this is frickin' fatal */
success = false;
goto out;
@ -204,7 +204,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false;
goto out;
}

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

@ -553,7 +553,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
else {
//Refuse to arm if preflight checks have failed
if(!status.condition_system_sensors_initialized) {
if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED;
break;
@ -1579,7 +1579,9 @@ int commander_thread_main(int argc, char *argv[])
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
if (armed.armed) {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
}
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
@ -1587,7 +1589,6 @@ int commander_thread_main(int argc, char *argv[])
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (!armed.armed) {
@ -1827,13 +1828,17 @@ int commander_thread_main(int argc, char *argv[])
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
/* handle the case where data link was gained first time or regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_info(mavlink_fd, "data link %i regained", i);
/* only report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
}
telemetry_lost[i] = false;
have_link = true;
@ -1844,10 +1849,12 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_info(mavlink_fd, "data link %i lost", i);
/* only reset the timestamp to a different time on state change */
telemetry_last_dl_loss[i] = hrt_absolute_time();
mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
telemetry_lost[i] = true;
}
}
@ -1862,7 +1869,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
@ -2756,8 +2763,6 @@ void *commander_low_prio_loop(void *arg)
// Update preflight check status
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (((int)(cmd.param1)) == 0) {
int ret = param_load_default();
@ -2781,9 +2786,15 @@ void *commander_low_prio_loop(void *arg)
}
} else if (((int)(cmd.param1)) == 1) {
int ret = param_save_default();
if (ret == OK) {
if (need_param_autosave) {
need_param_autosave = false;
need_param_autosave_timeout = 0;
}
mavlink_log_info(mavlink_fd, "settings saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);

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

@ -137,6 +137,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
prearm_ret = OK;
status->condition_system_sensors_initialized = true;
} else {
armed->lockdown = false;
@ -174,7 +176,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@ -184,7 +186,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
@ -211,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
@ -221,9 +223,9 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}

3
src/modules/mavlink/mavlink_messages.cpp
View File

@ -539,7 +539,8 @@ protected:
msg.errors_count2 = status.errors_count2;
msg.errors_count3 = status.errors_count3;
msg.errors_count4 = status.errors_count4;
msg.battery_remaining = status.battery_remaining * 100.0f;
msg.battery_remaining = (msg.voltage_battery > 0) ?
status.battery_remaining * 100.0f : -1;
_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
}

8
src/modules/sdlog2/sdlog2.c
View File

@ -1648,10 +1648,10 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention
log_msg.body.log_VISN.qx = buf.vision_pos.q[1];
log_msg.body.log_VISN.qy = buf.vision_pos.q[2];
log_msg.body.log_VISN.qz = buf.vision_pos.q[3];
LOGBUFFER_WRITE_AND_COUNT(VISN);
}

4
src/modules/uavcan/uavcan_main.cpp
View File

@ -698,7 +698,9 @@ int uavcan_main(int argc, char *argv[])
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
errx(1, "already started");
// Already running, no error
warnx("already started");
::exit(0);
}
// Node ID