diff options
| -rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 7 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 12 |
2 files changed, 10 insertions, 9 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 66c58f74f..1d4df87fe 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -418,9 +418,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.pitch = euler[1]; att.yaw = euler[2]; - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; + // XXX replace with x_apo after fix to filter + att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0]; + att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1]; + att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2]; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 57512bfd5..7d766bcdb 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -501,10 +501,10 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + current_status->flag_control_rates_enabled = false; // XXX if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -517,9 +517,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + current_status->flag_control_rates_enabled = false; // XXX if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -532,9 +532,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + current_status->flag_control_rates_enabled = false; // XXX if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { |