diff options
author | Julian Oes <julian@oes.ch> | 2014-02-10 11:09:09 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-02-10 11:09:09 +0100 |
commit | e0fbb0fb6079cffe1e3ab254caa4fd07906e9f7d (patch) | |
tree | 0e7b0a917145aca90924df1564c409cba4fabdf4 | |
parent | 3b2b270a40e0d8528339fe7cde5e0af91684fb97 (diff) | |
parent | 7baa78b113801fa939fe60d56641532cdeea29b9 (diff) | |
download | px4-firmware-e0fbb0fb6079cffe1e3ab254caa4fd07906e9f7d.tar.gz px4-firmware-e0fbb0fb6079cffe1e3ab254caa4fd07906e9f7d.tar.bz2 px4-firmware-e0fbb0fb6079cffe1e3ab254caa4fd07906e9f7d.zip |
Merge remote-tracking branch 'px4/beta' into acro2
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4001_quad_x | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.logging | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 18 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 18 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix | 7 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix | 7 | ||||
-rwxr-xr-x | nuttx-configs/px4fmu-v2/include/board.h | 4 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 25 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.cpp | 20 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.h | 21 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_load.c | 6 | ||||
-rw-r--r-- | src/systemcmds/mixer/mixer.cpp | 3 |
15 files changed, 74 insertions, 67 deletions
diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 42b64a68e..0f1288dec 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -48,5 +48,5 @@ set MIXER FMU_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 set PWM_DISARMED 900 -set PWM_MIN 1000 +set PWM_MIN 1100 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 270f51a58..fd5756586 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -9,4 +9,6 @@ sh /etc/init.d/4001_quad_x set MIXER FMU_hexa_x -set PWM_OUTPUTS 123456 +# We only can run one channel group with one rate, +# so all 8 at 400 Hz +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dcf5bbced..c5aef8273 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 16 -t + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 84dc7ec64..178bcaeba 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -474,7 +474,7 @@ then set MAV_TYPE 2 # Use mixer to detect vehicle type - if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix deleted file mode 100644 index f8f9f0e4d..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ /dev/null @@ -1,18 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the + configuration. All controls -are mixed 100%. - -R: 6+ 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix deleted file mode 100644 index 26b40b9e9..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ /dev/null @@ -1,18 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the X configuration. All controls -are mixed 100%. - -R: 6x 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix new file mode 100644 index 000000000..b5e38ce9e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the + configuration. All controls +are mixed 100%. + +R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix new file mode 100644 index 000000000..8e8d122ad --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the X configuration. All controls +are mixed 100%. + +R: 6x 10000 10000 10000 0 diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index e56b14ba4..850043ddf 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -209,8 +209,8 @@ #define GPIO_USART3_RX GPIO_USART3_RX_3 #define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 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 3ef1871a8..45fdaa355 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 @@ -236,7 +236,7 @@ private: float land_slope_length; float land_H1_virt; float land_flare_alt_relative; - float land_thrust_lim_horizontal_distance; + float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -281,7 +281,7 @@ private: param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; - param_t land_thrust_lim_horizontal_distance; + param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); - _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); - param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } @@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - - + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out @@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); + float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 7954d75c2..ee8721ff9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -172,5 +172,5 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index b139a6397..e5f7023ae 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -48,13 +48,13 @@ void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt) { _landing_slope_angle_rad = landing_slope_angle_rad; _flare_relative_alt = flare_relative_alt; - _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _motor_lim_relative_alt = motor_lim_relative_alt; _H1_virt = H1_virt; calculateSlopeValues(); @@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) { - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + else + return wp_altitude; +} +float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + else + return wp_landing_altitude; } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 1a149fc7c..76d65a55f 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -49,7 +49,7 @@ private: /* see Documentation/fw_landing.png for a plot of the landing slope */ float _landing_slope_angle_rad; /**< phi in the plot */ float _flare_relative_alt; /**< h_flare,rel in the plot */ - float _motor_lim_horizontal_distance; + float _motor_lim_relative_alt; float _H1_virt; /**< H1 in the plot */ float _H0; /**< h_flare,rel + H1 in the plot */ float _d1; /**< d1 in the plot */ @@ -63,7 +63,18 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); /** * @@ -85,17 +96,17 @@ public: } - float getFlareCurveAltitude(float wp_distance, float wp_altitude); + float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} inline float flare_relative_alt() {return _flare_relative_alt;} - inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;} inline float H1_virt() {return _H1_virt;} inline float H0() {return _H0;} inline float d1() {return _d1;} diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index a55ddf8a3..b05273c0d 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); if (fp == NULL) { - return 1; + warnx("file not found"); + return -1; } /* read valid lines from the file into a buffer */ @@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - return 1; + warnx("line too long"); + return -1; } /* add the line to the buffer */ diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 6da39d371..2f5ed3265 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,8 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0], sizeof(buf)); + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) + err(1, "can't load mixer: %s", fname); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); |