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

Minor fixes, pushing WIP

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-29 11:00:15 +01:00
parent be85589e48
commit c652f718c0
6 changed files with 60 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+29 -6
View File
@@ -147,7 +147,6 @@ int commander_thread_main(int argc, char *argv[]);
static int buzzer_init(void);
static void buzzer_deinit(void);
static void tune_confirm(void);
static int led_init(void);
static void led_deinit(void);
static int led_toggle(int led);
@@ -272,6 +271,10 @@ void tune_confirm(void) {
ioctl(buzzer, TONE_SET_ALARM, 3);
}
void tune_error(void) {
ioctl(buzzer, TONE_SET_ALARM, 4);
}
void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -980,8 +983,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break;
case MAV_CMD_PREFLIGHT_STORAGE: {
if (current_status.flag_system_armed) {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed");
if (current_status.flag_system_armed &&
((current_status.system_type == MAV_TYPE_QUADROTOR) ||
(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
(current_status.system_type == MAV_TYPE_OCTOROTOR))) {
/* do not perform expensive memory tasks on multirotors in flight */
// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
mavlink_log_info(mavlink_fd, "[cmd] REJECTING save cmd while multicopter armed");
} else {
// XXX move this to LOW PRIO THREAD of commander app
@@ -1680,6 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
* Check if manual control modes have to be switched
*/
if (!isfinite(sp_man.manual_mode_switch)) {
printf("man mode sw not finite\n");
/* this switch is not properly mapped, set default */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
@@ -1688,13 +1697,17 @@ int commander_thread_main(int argc, char *argv[])
/* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
}
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
} else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set direct mode for vehicles supporting it */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
@@ -1703,22 +1716,32 @@ int commander_thread_main(int argc, char *argv[])
/* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
/* assuming a fixed wing, set to direct pass-through as requested */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
}
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
} else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* center stick position, set rate control */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = true;
}
printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
/*
* Check if manual stability control modes have to be switched
*/
apps/commander/commander.h
+3
View File
@@ -52,4 +52,7 @@
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
void tune_confirm(void);
void tune_error(void);
#endif /* COMMANDER_H_ */
apps/commander/state_machine_helper.c
+1
View File
@@ -535,6 +535,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
{
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
tune_error();
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
apps/fixedwing_att_control/fixedwing_att_control_main.c
+2 -2
View File
@@ -201,7 +201,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
if (vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
@@ -244,7 +244,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
if (vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
apps/mavlink/mavlink.c
+24 -6
View File
@@ -189,10 +189,34 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode = 0;
/* set mode flags independent of system state */
/* HIL */
if (v_status.flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* manual input */
if (v_status.flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
/* attitude or rate control */
if (v_status.flag_control_attitude_enabled ||
v_status.flag_control_rates_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
/* vector control */
if (v_status.flag_control_velocity_enabled ||
v_status.flag_control_position_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
/* autonomous mode */
if (v_status.state_machine == SYSTEM_STATE_AUTO) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
/* set arming state */
if (armed.armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -221,20 +245,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case SYSTEM_STATE_MISSION_ABORT:
apps/sensors/sensors.cpp
+1 -17
View File
@@ -1088,24 +1088,8 @@ Sensors::ppm_poll()
manual_control.timestamp = rc_input.timestamp;
// /* check if input needs to be de-mixed */
// if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
// // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
// /* roll input - mixed roll and pitch channels */
// manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
// pitch input - mixed roll and pitch channels
// manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
// /* yaw input - stick right is positive and positive rotation */
// manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
// /* throttle input */
// manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
// /* direct pass-through of manual input */
// } else {
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.