diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-05 08:42:20 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-05 08:42:20 +0100 |
commit | d7064884dbba80514fee6946ead41613466fa571 (patch) | |
tree | 856e985c61794537867165cfae59cfcd2588e4a2 | |
parent | 5a5d758ce4dc72d3eb5809124134247c4cc0038f (diff) | |
parent | 1358d4cb88e70370014410353faf3e51afb1ceb6 (diff) | |
download | px4-firmware-d7064884dbba80514fee6946ead41613466fa571.tar.gz px4-firmware-d7064884dbba80514fee6946ead41613466fa571.tar.bz2 px4-firmware-d7064884dbba80514fee6946ead41613466fa571.zip |
Merge branch 'master' into fw_autoland
nored, and an empty message aborts
23 files changed, 259 insertions, 197 deletions
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix new file mode 100644 index 000000000..51cebb785 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls +are mixed 100%. + +R: 8c 10000 10000 10000 0 diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f9061c110..306827086 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ed2f2da5e..761fb8d9d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control @@ -120,7 +120,7 @@ MODULES += lib/conversion #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app +MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index efd6afb4b..51f916f37 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,9 +65,14 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** - * Minimum PWM in us + * Lowest minimum PWM in us */ -#define PWM_MIN 900 +#define PWM_LOWEST_MIN 900 + +/** + * Default minimum PWM in us + */ +#define PWM_DEFAULT_MIN 1000 /** * Highest PWM allowed as the minimum PWM @@ -75,9 +80,14 @@ __BEGIN_DECLS #define PWM_HIGHEST_MIN 1300 /** - * Maximum PWM in us + * Highest maximum PWM in us + */ +#define PWM_HIGHEST_MAX 2100 + +/** + * Default maximum PWM in us */ -#define PWM_MAX 2100 +#define PWM_DEFAULT_MAX 2000 /** * Lowest PWM allowed as the maximum PWM diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4bd27f907..0441566e9 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -232,8 +232,8 @@ PX4FMU::PX4FMU() : _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { - _min_pwm[i] = PWM_MIN; - _max_pwm[i] = PWM_MAX; + _min_pwm[i] = PWM_DEFAULT_MIN; + _max_pwm[i] = PWM_DEFAULT_MAX; } _debug_enabled = true; @@ -762,10 +762,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _failsafe_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _failsafe_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; } else { _failsafe_pwm[i] = pwm->values[i]; } @@ -801,10 +801,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _disarmed_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _disarmed_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; } else { _disarmed_pwm[i] = pwm->values[i]; } @@ -842,8 +842,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { _min_pwm[i] = PWM_HIGHEST_MIN; - } else if (pwm->values[i] < PWM_MIN) { - _min_pwm[i] = PWM_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; } else { _min_pwm[i] = pwm->values[i]; } @@ -872,8 +872,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_MAX) { - _max_pwm[i] = PWM_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; } else { _max_pwm[i] = pwm->values[i]; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 63e775857..08e697b9f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1985,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { + if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) { ret = -EINVAL; } else { diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 7f655964d..44e6dc7f3 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[]) int error_counter = 0; - while (true) { + for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4a98c8e97..f8f832ed7 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -54,7 +54,9 @@ public: _SPE_est(0.0f), _SKE_est(0.0f), _SPEdot(0.0f), - _SKEdot(0.0f) { + _SKEdot(0.0f), + _vel_dot(0.0f), + _STEdotErrLast(0.0f) { } diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp index 715fd3a5e..1945bb02d 100644 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ b/src/lib/mathlib/math/arm/Matrix.hpp @@ -121,10 +121,10 @@ public: for (size_t i = 0; i < getRows(); i++) { for (size_t j = 0; j < getCols(); j++) { float sig; - int exp; + int exponent; float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp index 4155800e8..52220fc15 100644 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ b/src/lib/mathlib/math/arm/Vector.hpp @@ -109,10 +109,10 @@ public: inline void print() const { for (size_t i = 0; i < getRows(); i++) { float sig; - int exp; + int exponent; float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 33879892e..18fb6dcbc 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -34,7 +34,7 @@ /** * @file KalmanNav.cpp * - * kalman filter navigation code + * Kalman filter navigation code */ #include <poll.h> @@ -228,10 +228,7 @@ void KalmanNav::update() updateSubscriptions(); // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate && - _sensors.accelerometer_counter > 10 && - _sensors.gyro_counter > 10 && - _sensors.magnetometer_counter > 10) { + if (!_attitudeInitialized && sensorsUpdate) { if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { @@ -643,7 +640,7 @@ int KalmanNav::correctAtt() if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:\n"); y.print(); + warnx("y:"); y.print(); } // update quaternions from euler diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 44a144296..ed090271c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); + + // XXX this would be the right approach to do it, but do we *WANT* this? + // /* disarm if safety is now on and still armed */ + // if (safety.safety_switch_available && !safety.safety_off) { + // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // } } /* update global position estimate */ @@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } @@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { @@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ - if (armed->armed) { + if (actuator_armed->armed) { /* armed, solid */ led_on(LED_BLUE); - } else if (armed->ready_to_arm) { + } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 09f4104f8..4ebf266f4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd) /* maximum 500 values */ const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; + unsigned int calibration_counter; struct mag_scale mscale_null = { 0.0f, @@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + mavlink_log_critical(mavlink_fd, "Skipped scale calibration"); + /* this is non-fatal - mark it accordingly */ + res = OK; } } close(fd); - float *x; - float *y; - float *z; + float *x = NULL; + float *y = NULL; + float *z = NULL; if (res == OK) { /* allocate memory */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = (float *)malloc(sizeof(float) * calibration_maxcount); - y = (float *)malloc(sizeof(float) * calibration_maxcount); - z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); res = ERROR; + return res; } + } else { + /* exit */ + return ERROR; } if (res == OK) { @@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_radius; if (res == OK) { + /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); @@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd) } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - - return res; } + + return res; } 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 eb67874db..00a0dcd61 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + if (isfinite(roll_sp) && isfinite(pitch_sp)) { - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); - rates_sp.timestamp = hrt_absolute_time(); + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } } } else { 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 1a263b741..536380129 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 @@ -470,7 +470,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); /* sanity check parameters */ @@ -520,7 +520,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); _airspeed_valid = true; _airspeed_last_valid = hrt_absolute_time(); - return true; } else { @@ -533,7 +532,7 @@ FixedwingPositionControl::vehicle_airspeed_poll() /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); - return false; + return airspeed_updated; } void diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 40597adf1..86a40bc22 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_ * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; /** * PAGE 108 @@ -278,10 +278,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values < PWM_MIN) { - r_page_servo_failsafe[offset] = PWM_MIN; - } else if (*values > PWM_MAX) { - r_page_servo_failsafe[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; } else { r_page_servo_failsafe[offset] = *values; } @@ -304,8 +304,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; - } else if (*values < PWM_MIN) { - r_page_servo_control_min[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -323,8 +323,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > PWM_MAX) { - r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { @@ -348,11 +348,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 723bf9f3b..1c889a811 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -449,6 +449,7 @@ public: HEX_PLUS, /**< hex in + configuration */ OCTA_X, OCTA_PLUS, + OCTA_COX, MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b89f341b6..bf77795d5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_octa_cox[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { 0.707107, -0.707107, 1.00 }, + { -0.707107, -0.707107, -1.00 }, + { 0.707107, 0.707107, 1.00 }, + { -0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, -0.707107, -1.00 }, +}; const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_hex_plus[0], &_config_octa_x[0], &_config_octa_plus[0], + &_config_octa_cox[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ @@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 6, /* hex_plus */ 8, /* octa_x */ 8, /* octa_plus */ + 8, /* octa_cox */ }; } @@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + + } else if (!strcmp(geomname, "8c")) { + geometry = MultirotorMixer::OCTA_COX; } else { debug("unrecognised geometry '%s'", geomname); diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 683c63040..050bf2f47 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -74,7 +74,18 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus} +set octa_cox { + 45 CCW + -45 CW + -135 CCW + 135 CW + -45 CCW + 45 CW + 135 CCW + -135 CW +} + +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus 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]]} diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 7abbf42ae..149b8f6d2 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp) } else { SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) + if (sd != nullptr) { + hrt_cancel(&sd->update_call); delete sd; + } } return CDev::close(filp); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 188dafa4e..80689f20c 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -272,7 +272,7 @@ do_accel(int argc, char *argv[]) } } else { - errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t"); + errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index d524ab23b..b237b31be 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -67,8 +67,6 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 14 - static void usage(const char *reason) { @@ -76,12 +74,15 @@ usage(const char *reason) warnx("%s", reason); errx(1, - "usage:\n" - "esc_calib [-l <low pwm>] [-h <high pwm>] [-d <device>] <channels>\n" - " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " <channels> Provide channels (e.g.: 1 2 3 4)\n" - ); - + "usage:\n" + "esc_calib\n" + " [-d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " [-l <pwm> Low PWM value in us (default: %dus)\n" + " [-h <pwm> High PWM value in us (default: %dus)\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Use all outputs\n" + , PWM_DEFAULT_MIN, PWM_DEFAULT_MAX); } int @@ -89,13 +90,18 @@ esc_calib_main(int argc, char *argv[]) { char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; - bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; - int low = -1; - int high = -1; + unsigned max_channels = 0; + + uint32_t set_mask = 0; + unsigned long channels; + unsigned single_ch = 0; + + uint16_t pwm_high = PWM_DEFAULT_MAX; + uint16_t pwm_low = PWM_DEFAULT_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -107,7 +113,7 @@ esc_calib_main(int argc, char *argv[]) int arg_consumed = 0; - while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) { + while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { switch (ch) { case 'd': @@ -115,41 +121,49 @@ esc_calib_main(int argc, char *argv[]) arg_consumed += 2; break; - case 'l': - low = strtoul(optarg, &ep, 0); - if (*ep != '\0') - usage("bad low pwm value"); - arg_consumed += 2; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } break; - case 'h': - high = strtoul(optarg, &ep, 0); + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad high pwm value"); - arg_consumed += 2; + usage("bad set_mask value"); + break; + + case 'a': + /* Choose all channels */ + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + set_mask |= 1<<i; + } break; + case 'l': + /* Read in custom low value */ + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) + usage("low PWM invalid"); + break; + case 'h': + /* Read in custom high value */ + pwm_high = strtoul(optarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) + usage("high PWM invalid"); + break; default: usage(NULL); } } - while ((--argc - arg_consumed) > 0) { - const char *arg = argv[argc]; - unsigned channel_number = strtol(arg, &ep, 0); - - warnx("adding channel #%d", channel_number); - - if (*ep == '\0') { - if (channel_number > MAX_CHANNELS || channel_number <= 0) { - err(1, "invalid channel number: %d", channel_number); - - } else { - channels_selected[channel_number - 1] = true; - - } - } - } + if (set_mask == 0) + usage("no channels chosen"); /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; @@ -217,50 +231,10 @@ esc_calib_main(int argc, char *argv[]) if (fd < 0) err(1, "can't open %s", dev); - /* get max PWM value setting */ - uint16_t pwm_max = 0; - - if (high > 0 && high > low && high < 2200) { - pwm_max = high; - } else { - ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MAX_PWM"); - } - - /* bound to sane values */ - if (pwm_max > 2200) - pwm_max = 2200; - - if (pwm_max < 1700) - pwm_max = 1700; - - /* get disarmed PWM value setting */ - uint16_t pwm_disarmed = 0; - - if (low > 0 && low < high && low > 800) { - pwm_disarmed = low; - } else { - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_DISARMED_PWM"); - - if (pwm_disarmed == 0) { - ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MIN_PWM"); - } - } - - /* bound to sane values */ - if (pwm_disarmed > 1300) - pwm_disarmed = 1300; - - if (pwm_disarmed < 800) - pwm_disarmed = 800; + /* get number of channels available on the device */ + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); @@ -273,21 +247,23 @@ esc_calib_main(int argc, char *argv[]) warnx("Outputs armed"); + /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" "\n" "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" - "\n", pwm_max); + "\n", pwm_high); fflush(stdout); while (1) { /* set max PWM */ - for (unsigned i = 0; i < MAX_CHANNELS; i++) { - if (channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); + for (unsigned i = 0; i < max_channels; i++) { + + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high); if (ret != OK) - err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_max); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_high); } } @@ -314,17 +290,17 @@ esc_calib_main(int argc, char *argv[]) printf("Low PWM set: %d\n" "\n" "Hit ENTER when finished\n" - "\n", pwm_disarmed); + "\n", pwm_low); while (1) { /* set disarmed PWM */ - for (unsigned i = 0; i < MAX_CHANNELS; i++) { - if (channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed); + for (unsigned i = 0; i < max_channels; i++) { + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low); if (ret != OK) - err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_disarmed); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_low); } } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 1c58a2db6..07581459b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- ACCEL ---- */ close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { @@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + /* ---- GYRO ---- */ close(fd); @@ -193,9 +216,10 @@ system_eval: /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - /* switch on leds */ + /* switch off leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); + close(leds); if (fail_on_error) { /* exit with error message */ |