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

This code is flyable, but a few problems exist which cause values in the .config files to be overwritten by the defaults in the .XML file.

Browse Source
This commit is contained in:
jwilson
2016-02-12 21:39:50 -08:00
committed by Julian Oes
parent 8cc37db595
commit d84639f8c4
7 changed files with 171 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+55 -14
View File
@@ -724,10 +724,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Refuse to arm if preflight checks have failed
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_command_s::VEHICLE_CMD_RESULT_DENIED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -1155,7 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
status.condition_system_prearm_error_reported = false;
status.condition_system_hotplug_timeout = false;
@@ -2196,7 +2196,11 @@ int commander_thread_main(int argc, char *argv[])
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
#else
// XXX HACK: remove old data check due to timestamp issue in QURT
// TODO-JYW: TESTING-TESTING
warnx("status.rc_input_blocked: %d, sp_man.timestamp: %d", status.rc_input_blocked, sp_man.timestamp);
// TODO-JYW: TESTING-TESTING
// HACK: remove old data check due to timestamp issue in QURT
if (!status.rc_input_blocked && sp_man.timestamp != 0) {
#endif
/* handle the case where RC signal was regained */
@@ -2214,6 +2218,14 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
// TODO-JYW: TESTING-TESTING
warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d",
status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d, status.main_state: %d, status.condition_landed: %d, sp_man.r: %f, sp_man.z: %f",
status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
// TODO-JYW: TESTING-TESTING
/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
@@ -2246,6 +2258,13 @@ int commander_thread_main(int argc, char *argv[])
stick_off_counter = 0;
}
// TODO-JYW: TESTING-TESTING
warnx("lower right position: ON_OFF_LIMIT: %f, stick_off_counter: %d, stick_on_counter: %d",
(double)STICK_ON_OFF_LIMIT, stick_off_counter, stick_on_counter);
warnx("lower right position: sp_man.r: %.6f, sp_man.z: %.6f, status.rc_input_mode: %d",
(double)sp_man.r, (double)sp_man.z, status.rc_input_mode);
// TODO-JYW: TESTING-TESTING
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -2257,23 +2276,32 @@ int commander_thread_main(int argc, char *argv[])
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
// TOOD-JYW: TESTING-TESTING:
warnx("NOT ARMING: Switch to MANUAL mode first.");
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else if (!status.condition_home_position_valid &&
geofence_action == geofence_result_s::GF_ACTION_RTL) {
// TOOD-JYW: TESTING-TESTING:
warnx("NOT ARMING: Geofence RTL requires valid home");
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
// TODO-JYW: TESTING-TESTING
warnx("attempting arming_state_transition");
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
} else {
// TOOD-JYW: TESTING-TESTING:
warnx("NOT ARMING: Configuration error");
print_reject_arm("NOT ARMING: Configuration error");
}
}
// TOOD-JYW: TESTING-TESTING:
warnx("on counter set to zero");
stick_on_counter = 0;
} else {
@@ -2281,9 +2309,14 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
// TOOD-JYW: TESTING-TESTING:
warnx("on counter set to zero");
stick_on_counter = 0;
}
// TOOD-JYW: TESTING-TESTING:
warnx("arming_ret: %d, arming_state_changed: %d, status.main_state: %d, status.arming_state: %d", arming_ret, arming_state_changed, status.main_state, status.arming_state);
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
@@ -2579,6 +2612,8 @@ int commander_thread_main(int argc, char *argv[])
*/
armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
}
// TODO-JYW: TESTING-TESTING:
warnx("publishing arming status, armed.armed: %d", armed.armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -2602,7 +2637,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
@@ -2613,16 +2648,16 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
status_changed = true;
}
/* update timeout flag */
if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
status.condition_system_hotplug_timeout = hotplug_timeout;
@@ -2714,7 +2749,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (changed) {
bool set_normal_color = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* set mode */
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
@@ -2727,7 +2762,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2876,9 +2911,9 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
}
}
else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
/* Similar to acro transitions for multirotors. FW aircraft don't need a
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
@@ -3165,6 +3200,12 @@ set_control_mode()
default:
break;
}
// TODO-JYW: TESTING-TESTING:
warnx("status.nav_state: %d", status.nav_state);
warnx("status.is_rotary_wing: %d", status.is_rotary_wing);
warnx("control_mode.flag_control_rates_enabled: %d", control_mode.flag_control_rates_enabled);
warnx("control_mode.flag_control_attitude_enabled: %d", control_mode.flag_control_attitude_enabled);
}
bool
@@ -3398,7 +3439,7 @@ void *commander_low_prio_loop(void *arg)
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_esc_calibration(mavlink_fd, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {