diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-21 21:53:26 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-21 21:53:26 +0200 |
commit | 8db5c932f97d7e6d554fd7e848c170f7e7093a8c (patch) | |
tree | 9fac5827025ebba21d08155c738654afa530b04a /src/modules | |
parent | a28a38005c6c9fdd43e6e7182b93d274d4f4369c (diff) | |
parent | b9b81beb17eb449921f11f46bc419056dce03852 (diff) | |
download | px4-firmware-8db5c932f97d7e6d554fd7e848c170f7e7093a8c.tar.gz px4-firmware-8db5c932f97d7e6d554fd7e848c170f7e7093a8c.tar.bz2 px4-firmware-8db5c932f97d7e6d554fd7e848c170f7e7093a8c.zip |
Merge branch 'fwattrobustify' into mtecs
Diffstat (limited to 'src/modules')
29 files changed, 279 insertions, 61 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 2ec889efe..66a949af1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -266,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; @@ -274,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; @@ -287,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* initialize parameter handles */ parameters_init(&ekf_param_handles); - uint64_t start_time = hrt_absolute_time(); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; @@ -382,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -393,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -445,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -498,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 504696ff9..c26200048 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -453,6 +453,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { + /* ACRO */ + main_res = main_state_transition(status, MAIN_STATE_ACRO); } } else { @@ -652,6 +656,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; + main_states_str[4] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1012,7 +1017,26 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); + /* hysteresis for EPH */ + bool local_eph_good; + + if (status.condition_global_position_valid) { + if (local_position.eph > eph_epv_threshold * 2.0f) { + local_eph_good = false; + + } else { + local_eph_good = true; + } + + } else { + if (local_position.eph < eph_epv_threshold) { + local_eph_good = true; + + } else { + local_eph_good = false; + } + } + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; @@ -1187,7 +1211,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1600,7 +1624,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_OFF: // MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (sp_man->acro_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_ACRO); + + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1712,6 +1741,17 @@ set_control_mode() navigator_enabled = true; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + default: break; } diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index a83c81850..e0f8dc95d 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -15,6 +15,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_ALTCTL, PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index abcf72717..f1a353d5b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -222,6 +222,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7a71894ed..0a6d3fa8f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1067,7 +1067,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index cafce4417..178b590ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -134,6 +134,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ @@ -304,12 +306,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _actuators_0_pub(-1), _attitude_sp_pub(-1), + _actuators_0_pub(-1), _actuators_1_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false) { @@ -387,6 +391,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() } while (_control_task != -1); } + perf_free(_loop_perf); + perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_output_perf); + att_control::g_control = nullptr; } @@ -674,10 +682,12 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (!isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; - + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } } else { airspeed = _airspeed.true_airspeed_m_s; } @@ -778,8 +788,10 @@ FixedwingAttitudeControl::task_main() _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (loop_counter % 10 == 0) { - warnx("roll_u %.4f", roll_u); + warnx("roll_u %.4f", (double)roll_u); } } @@ -790,6 +802,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," @@ -813,6 +826,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -826,6 +840,7 @@ FixedwingAttitudeControl::task_main() } } } else { + perf_count(_nonfinite_input_perf); if (loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 95165ecaa..4f40682ea 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -434,8 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), - last_manual(false), usePreTakeoffThrust(false), + last_manual(false), flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca7..28dd97fca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -149,10 +149,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - } - - /* no valid instance, bail */ - if (!instance) { + default: return; } @@ -211,9 +208,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - next(nullptr), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), + next(nullptr), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -234,7 +231,6 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), - /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { @@ -2030,14 +2026,14 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, _device_name); + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); } delete _subscribe_to_stream; @@ -2243,7 +2239,6 @@ Mavlink::stream(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; - int ch; argc -= 2; argv += 2; @@ -2280,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) i++; } - if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c7a7d32f8..25c0da820 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -221,8 +221,6 @@ private: int _mavlink_fd; bool _task_running; - perf_counter_t _loop_perf; /**< loop performance counter */ - /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ @@ -282,7 +280,7 @@ private: pthread_mutex_t _message_buffer_mutex; - + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Send one parameter. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 79dd88657..933478f56 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -136,6 +136,10 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + + } else if (status->main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } } else { diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b..21d5219d3 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const orb_id_t +orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5c6543e81..8c09772c8 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -63,7 +63,7 @@ public: */ bool is_published(); void *get_data(); - const orb_id_t get_topic(); + orb_id_t get_topic(); private: const orb_id_t _topic; /*< topic metadata */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..53769e0cf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context) rcv->receive_thread(NULL); delete rcv; + + return nullptr; } pthread_t diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index bb19d7e33..5ec30bd33 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t) /* interval expired, send message */ send(t); _last_sent = (t / _interval) * _interval; + + return 0; } + + return -1; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index def40d9ad..2979d20de 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -63,9 +63,13 @@ public: MavlinkStream *next; MavlinkStream(); - ~MavlinkStream(); + virtual ~MavlinkStream(); void set_interval(const unsigned int interval); void set_channel(mavlink_channel_t channel); + + /** + * @return 0 if updated / sent, -1 if unchanged + */ int update(const hrt_abstime t); virtual MavlinkStream *new_instance() = 0; virtual void subscribe(Mavlink *mavlink) = 0; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b23166a5e..20e016da3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -162,6 +162,9 @@ private: param_t man_roll_max; param_t man_pitch_max; param_t man_yaw_max; + param_t acro_roll_max; + param_t acro_pitch_max; + param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -175,6 +178,7 @@ private: float man_roll_max; float man_pitch_max; float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; /** @@ -284,6 +288,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); _rates_prev.zero(); _rates_sp.zero(); @@ -310,6 +315,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); + _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -386,6 +394,14 @@ MulticopterAttitudeControl::parameters_update() _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); + /* acro control scale */ + param_get(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + param_get(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + param_get(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + return OK; } @@ -782,11 +798,36 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp.z; + + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; + } } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index ad38a0a03..819847b40 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,3 +215,32 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); + +/** + * Max acro roll rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); + +/** + * Max acro pitch rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); + +/** + * Max acro yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d8d0ff37d..fdc3233e0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float eph = 1.0; + float epv = 1.0; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_valid = true; + eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + } else { w_flow = 0.0f; flow_valid = false; @@ -673,6 +678,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + eph = fminf(eph, gps.eph_m); + epv = fminf(epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -712,6 +720,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; + /* increase EPH/EPV on each step */ + eph *= 1.0 + dt; + epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; @@ -723,7 +735,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + bool can_estimate_xy = eph < max_eph_epv * 1.5; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -922,6 +934,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.landed = landed; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; + local_pos.eph = eph; + local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; @@ -950,9 +964,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; - // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = eph; + global_pos.epv = epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39f433eb5..70ce76806 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1119,10 +1119,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | + (buf.local_pos.z_valid ? 2 : 0) | + (buf.local_pos.v_xy_valid ? 4 : 0) | + (buf.local_pos.v_z_valid ? 8 : 0) | + (buf.local_pos.xy_global ? 16 : 0) | + (buf.local_pos.z_global ? 32 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + log_msg.body.log_LPOS.eph = buf.local_pos.eph; + log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0c6188657..90025b9ff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,10 +109,11 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - uint8_t xy_flags; - uint8_t z_flags; + uint8_t pos_flags; uint8_t landed; uint8_t ground_dist_flags; + float eph; + float epv; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -360,7 +361,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c021e1fde..999cf8bb3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -626,6 +626,15 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); /** + * Acro switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); + +/** * Flaps channel mapping. * * @min 0 @@ -756,3 +765,19 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); + +/** + * Threshold for selecting acro mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c5954d02a..b268b1b36 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,6 +263,7 @@ private: int rc_map_return_sw; int rc_map_posctl_sw; int rc_map_loiter_sw; + int rc_map_acro_sw; int rc_map_flaps; @@ -278,11 +279,13 @@ private: float rc_posctl_th; float rc_return_th; float rc_loiter_th; + float rc_acro_th; bool rc_assist_inv; bool rc_auto_inv; bool rc_posctl_inv; bool rc_return_inv; bool rc_loiter_inv; + bool rc_acro_inv; float battery_voltage_scaling; float battery_current_scaling; @@ -315,6 +318,7 @@ private: param_t rc_map_return_sw; param_t rc_map_posctl_sw; param_t rc_map_loiter_sw; + param_t rc_map_acro_sw; param_t rc_map_flaps; @@ -330,6 +334,7 @@ private: param_t rc_posctl_th; param_t rc_return_th; param_t rc_loiter_th; + param_t rc_acro_th; param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -530,6 +535,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -544,6 +550,7 @@ Sensors::Sensors() : _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -687,6 +694,10 @@ Sensors::parameters_update() warnx("%s", paramerr); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("%s", paramerr); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("%s", paramerr); } @@ -712,6 +723,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); + param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); + _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); + _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -723,6 +737,7 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1508,6 +1523,7 @@ Sensors::rc_poll() manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index fa9d28b74..225570fa4 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -447,6 +447,7 @@ public: QUAD_WIDE, /**< quad in wide configuration */ HEX_X, /**< hex in X configuration */ HEX_PLUS, /**< hex in + configuration */ + HEX_COX, OCTA_X, OCTA_PLUS, OCTA_COX, diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 1ca0a21e9..4ad21d818 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -115,6 +115,14 @@ const MultirotorMixer::Rotor _config_hex_plus[] = { { 0.866025, 0.500000, 1.00 }, { -0.866025, -0.500000, -1.00 }, }; +const MultirotorMixer::Rotor _config_hex_cox[] = { + { -0.866025, 0.500000, -1.00 }, + { -0.866025, 0.500000, 1.00 }, + { -0.000000, -1.000000, -1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 0.866025, 0.500000, -1.00 }, + { 0.866025, 0.500000, 1.00 }, +}; const MultirotorMixer::Rotor _config_octa_x[] = { { -0.382683, 0.923880, -1.00 }, { 0.382683, -0.923880, -1.00 }, @@ -152,6 +160,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_wide[0], &_config_hex_x[0], &_config_hex_plus[0], + &_config_hex_cox[0], &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], @@ -163,6 +172,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_wide */ 6, /* hex_x */ 6, /* hex_plus */ + 6, /* hex_cox */ 8, /* octa_x */ 8, /* octa_plus */ 8, /* octa_cox */ @@ -252,6 +262,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "6x")) { geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "6c")) { + geometry = MultirotorMixer::HEX_COX; + } else if (!strcmp(geomname, "8+")) { geometry = MultirotorMixer::OCTA_PLUS; diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 050bf2f47..b5698036e 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -52,6 +52,15 @@ set hex_plus { 120 CW } +set hex_cox { + 60 CW + 60 CCW + 180 CW + 180 CCW + -60 CW + -60 CCW +} + set octa_x { 22.5 CW -157.5 CW @@ -85,7 +94,7 @@ set octa_cox { -135 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox} +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} @@ -104,13 +113,13 @@ foreach table $tables { puts "};" } -puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {" foreach table $tables { puts [format "\t&_config_%s\[0\]," $table] } puts "};" -puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {" foreach table $tables { upvar #0 $table angles puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 19a29635b..910b8a623 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -97,6 +97,7 @@ struct manual_control_setpoint_s { switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index b60402452..370c54442 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -45,12 +45,12 @@ /** * The number of RC channel inputs supported. * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 15. + * leaving at a sane value of 16. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAPPED_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 16 /** * This defines the mapping of the RC functions. @@ -67,12 +67,13 @@ enum RC_CHANNELS_FUNCTION { POSCTL = 6, LOITER = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index a15303ea4..5d39c897d 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -83,6 +83,8 @@ struct vehicle_local_position_s { float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ + float eph; + float epv; }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f56355246..ea20a317a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,6 +66,7 @@ typedef enum { MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, MAIN_STATE_AUTO, + MAIN_STATE_ACRO, MAIN_STATE_MAX } main_state_t; |