diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-29 11:00:15 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-29 11:00:15 +0100 |
commit | c652f718c0d2ab78fd80f503d5932ecf526136a9 (patch) | |
tree | 763656259754c8c786ae1577643eecf7d895d9b7 | |
parent | be85589e48cdc1b297bffe8e492cd7d69f949b4b (diff) | |
download | px4-firmware-c652f718c0d2ab78fd80f503d5932ecf526136a9.tar.gz px4-firmware-c652f718c0d2ab78fd80f503d5932ecf526136a9.tar.bz2 px4-firmware-c652f718c0d2ab78fd80f503d5932ecf526136a9.zip |
Minor fixes, pushing WIP
-rw-r--r-- | apps/commander/commander.c | 35 | ||||
-rw-r--r-- | apps/commander/commander.h | 3 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 1 | ||||
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 4 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 30 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 18 |
6 files changed, 60 insertions, 31 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 50544320b..be50289c3 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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 */ diff --git a/apps/commander/commander.h b/apps/commander/commander.h index bea67bead..04b4e72ab 100644 --- a/apps/commander/commander.h +++ b/apps/commander/commander.h @@ -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_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 4152142df..a8cef8c01 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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) { diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 5ededa2e3..c51d06579 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -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); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 3351d9cfd..ccc9d6d7d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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: diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 10d25d347..c331ec3e3 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -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. |