diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 20 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 21 | ||||
-rw-r--r-- | src/modules/controllib/blocks.cpp | 13 | ||||
-rw-r--r-- | src/modules/controllib/blocks.hpp | 17 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 63 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 176 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 10 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 9 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 26 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 3 | ||||
-rw-r--r-- | src/modules/uORB/topics/actuator_armed.h | 3 | ||||
-rw-r--r-- | src/modules/uORB/topics/position_setpoint_triplet.h | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/tecs_status.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_command.h | 1 | ||||
-rw-r--r-- | src/systemcmds/pwm/pwm.c | 2 |
18 files changed, 97 insertions, 280 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c6ee08fbe..d93009c47 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } + if (armed.force_failsafe) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2437,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { - /* check IO firmware CRC against passed value */ + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); if (ret != OK) @@ -2697,7 +2703,7 @@ checkcrc(int argc, char *argv[]) int fd = open(argv[1], O_RDONLY); if (fd == -1) { printf("open of %s failed - %d\n", argv[1], errno); - exit(1); + exit(1); } const uint32_t app_size_max = 0xf000; uint32_t fw_crc = 0; @@ -2712,7 +2718,7 @@ checkcrc(int argc, char *argv[]) close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); + fw_crc = crc32part(&b, 1, fw_crc); nbytes++; } @@ -2725,7 +2731,7 @@ checkcrc(int argc, char *argv[]) if (ret != OK) { printf("check CRC failed - %d\n", ret); - exit(1); + exit(1); } printf("CRCs match\n"); exit(0); @@ -2755,12 +2761,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2962,7 +2968,7 @@ lockdown(int argc, char *argv[]) (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); warnx("ACTUATORS ARE NOW SAFE IN HIL."); } - + } else { errx(1, "driver not loaded, exiting"); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a..04450a44f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; -#if 0 /* Flight termination */ - case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - - //XXX: to enable the parachute, a param needs to be set - //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { - transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - + case VEHICLE_CMD_DO_FLIGHTTERMINATION: { + if (cmd->param1 > 0.5f) { + //XXX update state machine? + armed_local->force_failsafe = true; + warnx("forcing failsafe"); } else { - /* reject parachute depoyment not armed */ - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + armed_local->force_failsafe = false; + warnx("disabling failsafe"); } - + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; -#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index c6c374300..0175acda9 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,7 +293,18 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = _lowPass.update((input - getU()) / getDt()); + float output; + if (_initialized) { + output = _lowPass.update((input - getU()) / getDt()); + } else { + // if this is the first call to update + // we have no valid derivative + // and so we use the assumption the + // input value is not changing much, + // which is the best we can do here. + output = 0.0f; + _initialized = true; + } setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 66e929038..37d7832b3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -238,9 +238,25 @@ public: BlockDerivative(SuperBlock *parent, const char *name) : SuperBlock(parent, name), _u(0), + _initialized(false), _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ float update(float input); // accessors void setU(float u) {_u = u;} @@ -249,6 +265,7 @@ public: protected: // attributes float _u; /**< previous input */ + bool _initialized; BlockLowPass _lowPass; /**< low pass filter */ }; 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 6c251c237..0b111f7bd 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 @@ -199,19 +199,6 @@ private: float l1_period; float l1_damping; - float time_const; - float min_sink_rate; - float max_sink_rate; - float max_climb_rate; - float throttle_damp; - float integrator_gain; - float vertical_accel_limit; - float height_comp_filter_omega; - float speed_comp_filter_omega; - float roll_throttle_compensation; - float speed_weight; - float pitch_damping; - float airspeed_min; float airspeed_trim; float airspeed_max; @@ -225,9 +212,6 @@ private: float throttle_land_max; - float heightrate_p; - float speedrate_p; - float land_slope_angle; float land_H1_virt; float land_flare_alt_relative; @@ -242,19 +226,6 @@ private: param_t l1_period; param_t l1_damping; - param_t time_const; - param_t min_sink_rate; - param_t max_sink_rate; - param_t max_climb_rate; - param_t throttle_damp; - param_t integrator_gain; - param_t vertical_accel_limit; - param_t height_comp_filter_omega; - param_t speed_comp_filter_omega; - param_t roll_throttle_compensation; - param_t speed_weight; - param_t pitch_damping; - param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -268,9 +239,6 @@ private: param_t throttle_land_max; - param_t heightrate_p; - param_t speedrate_p; - param_t land_slope_angle; param_t land_H1_virt; param_t land_flare_alt_relative; @@ -470,21 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); - _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); - _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); - _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); - _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); - _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); - _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); - _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); - _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); - _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); - _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); - _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); - _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); - _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); - /* fetch initial parameter values */ parameters_update(); } @@ -535,22 +488,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); - param_get(_parameter_handles.time_const, &(_parameters.time_const)); - param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); - param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); - param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); - param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); - param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); - param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); - param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); - param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); - param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); - param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); - param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); - - param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); - param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); - param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); 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 52128e1b7..27039ff51 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 @@ -155,182 +155,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); /** - * Maximum climb rate - * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or - * FW_THR_MAX reduced. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - -/** - * Minimum descent rate - * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used - * to measure FW_T_CLMB_MAX. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); - -/** - * Maximum descent rate - * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding - * the aircraft. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/** - * TECS time constant - * - * This is the time constant of the TECS control algorithm (in seconds). - * Smaller values make it faster to respond, larger values make it slower - * to respond. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/** - * Throttle damping factor - * - * This is the damping gain for the throttle demand loop. - * Increase to add damping to correct for oscillations in speed and height. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - -/** - * Integrator gain - * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and - * increases overshoot. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - -/** - * Maximum vertical acceleration - * - * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover - * from under-speed conditions. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/** - * Complementary filter "omega" parameter for height - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights - * the solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - -/** - * Complementary filter "omega" parameter for speed - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an - * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the - * solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - -/** - * Roll -> Throttle feedforward - * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas - * inefficient low aspect-ratio models (eg delta wings) can use a higher value. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - -/** - * Speed <--> Altitude priority - * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will - * adjust its pitch angle to maintain airspeed, ignoring changes in height). - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - -/** - * Pitch damping factor - * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned - * properly. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -/** - * Height rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); - -/** - * Speed rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); - -/** * Landing slope angle * * @group L1 Control diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 2e37d166e..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,6 +59,7 @@ mTecs::mTecs() : _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), _flightPathAngleLowpass(this, "FPA_LP"), + _altitudeLowpass(this, "ALT_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* time measurement */ updateTimeMeasurement(); + /* Filter altitude */ + float altitudeFiltered = _altitudeLowpass.update(altitude); + + /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } /* Write part of the status message */ _status.altitudeSp = altitudeSp; _status.altitude = altitude; + _status.altitudeFiltered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index efa89a5d3..ae6867d38 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -115,6 +115,7 @@ protected: /* Other calculation Blocks */ control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 5b9238780..4ca31fe20 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** + * Lowpass (cutoff freq.) for altitude + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); + +/** * Lowpass (cutoff freq.) for the flight path angle * * @group mTECS diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c801..606521f20 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 93cbfd7b1..331a9a728 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -458,7 +458,7 @@ void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - /* TODO: set nav_state */ + _pos_sp_triplet.nav_state = _vstatus.nav_state; /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4c..31861c3fc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); + + if (buf.triplet.current.valid) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } } /* --- VICON POSITION --- */ @@ -1595,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b14ef04cc..853a3811f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -334,6 +334,7 @@ struct log_GS1B_s { struct log_TECS_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; @@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index a98d3fc3a..0f6c9aca1 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -56,6 +56,7 @@ struct actuator_armed_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ }; /** @@ -65,4 +66,4 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 673c0e491..4a1932180 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -95,6 +95,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + unsigned nav_state; /**< report the navigation state */ }; /** diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index c4d0c1874..33055018c 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -66,6 +66,7 @@ struct tecs_status_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b13..7db33d98b 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -76,6 +76,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 742f45484..403567cb4 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -70,7 +70,7 @@ usage(const char *reason) { if (reason != NULL) warnx("%s", reason); - errx(1, + errx(1, "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" |