diff options
Diffstat (limited to 'src/modules')
30 files changed, 987 insertions, 214 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc0594bf2..086f291f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1267,7 +1267,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); - + status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; @@ -2246,8 +2246,8 @@ set_control_mode() case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; 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 c0b1bb404..968270847 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 @@ -1471,7 +1471,7 @@ FixedwingEstimator::task_main() map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.time_utc_usec = _gps.time_utc_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } 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 a0f74517c..00e8393bf 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -336,8 +336,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_0_pub(-1), _actuators_2_pub(-1), - _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), - _actuators_id(ORB_ID(actuator_controls_0)), + _rates_sp_id(0), + _actuators_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -589,12 +589,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_fw); - } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); + if (!_rates_sp_id) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } } @@ -720,22 +722,24 @@ FixedwingAttitudeControl::task_main() R.set(_att.R); R_adapted.set(_att.R); - //move z to x + /* move z to x */ R_adapted(0, 0) = R(0, 2); R_adapted(1, 0) = R(1, 2); R_adapted(2, 0) = R(2, 2); - //move x to z + + /* move x to z */ R_adapted(0, 2) = R(0, 0); R_adapted(1, 2) = R(1, 0); R_adapted(2, 2) = R(2, 0); - //change direction of pitch (convert to right handed system) + /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); - //fill in new attitude data + + /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); @@ -749,7 +753,7 @@ FixedwingAttitudeControl::task_main() PX4_R(_att.R, 2, 1) = R_adapted(2, 1); PX4_R(_att.R, 2, 2) = R_adapted(2, 2); - // lastly, roll- and yawspeed have to be swaped + /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; @@ -820,6 +824,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if @@ -880,6 +885,8 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + /* allow manual control of rudder deflection */ + yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -979,6 +986,9 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + + /* add in manual rudder control */ + _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1018,7 +1028,7 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); - } else { + } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } @@ -1043,7 +1053,7 @@ FixedwingAttitudeControl::task_main() /* publish the actuator controls */ if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else { + } else if (_actuators_id) { _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 89c6494c5..3661a171f 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -41,3 +41,5 @@ SRCS = fw_att_control_main.cpp \ fw_att_control_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f9a3681b3..dc031404d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1419,6 +1419,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); configure_stream("VFR_HUD", 10.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("TIMESYNC", 10.0f); break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0f25c969d..6765100c7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -342,6 +342,8 @@ private: MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; + unsigned write_err_count = 0; + static const unsigned write_err_threshold = 5; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) @@ -370,10 +372,21 @@ protected: /* write log messages in first instance to disk */ if (_mavlink->get_instance_id() == 0) { if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { + if (EOF == fputs(msg.text, fp)) { + write_err_count++; + } else { + write_err_count = 0; + } + + if (write_err_count >= write_err_threshold) { + (void)fclose(fp); + fp = nullptr; + } else { + (void)fputs("\n", fp); + (void)fsync(fileno(fp)); + } + + } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; char log_file_path[64] = ""; @@ -389,6 +402,10 @@ protected: strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); + + /* write first message */ + fputs(msg.text, fp); + fputs("\n", fp); } } } @@ -923,6 +940,92 @@ protected: } }; +class MavlinkStreamSystemTime : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamSystemTime::get_name_static(); + } + + static const char *get_name_static() { + return "SYSTEM_TIME"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_SYSTEM_TIME; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamSystemTime(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamSystemTime(MavlinkStreamSystemTime &); + MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &); + +protected: + explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_system_time_t msg; + timespec tv; + + clock_gettime(CLOCK_REALTIME, &tv); + + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + _mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); + } +}; + +class MavlinkStreamTimesync : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamTimesync::get_name_static(); + } + + static const char *get_name_static() { + return "TIMESYNC"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_TIMESYNC; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamTimesync(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamTimesync(MavlinkStreamTimesync &); + MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &); + +protected: + explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_timesync_t msg; + + msg.tc1 = 0; + msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); + } +}; class MavlinkStreamGlobalPositionInt : public MavlinkStream { @@ -2197,6 +2300,8 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static), + new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index cd5f53d88..e9858b73c 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -44,7 +44,9 @@ #include "mavlink_main.h" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), - _send_all_index(-1) + _send_all_index(-1), + _rc_param_map_pub(-1), + _rc_param_map() { } @@ -135,6 +137,43 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) break; } + case MAVLINK_MSG_ID_PARAM_MAP_RC: { + /* map a rc channel to a parameter */ + mavlink_param_map_rc_t map_rc; + mavlink_msg_param_map_rc_decode(msg, &map_rc); + + if (map_rc.target_system == mavlink_system.sysid && + (map_rc.target_component == mavlink_system.compid || + map_rc.target_component == MAV_COMP_ID_ALL)) { + + /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ + size_t i = map_rc.parameter_rc_channel_index; + _rc_param_map.param_index[i] = map_rc.param_index; + strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.scale[i] = map_rc.scale; + _rc_param_map.value0[i] = map_rc.param_value0; + _rc_param_map.value_min[i] = map_rc.param_value_min; + _rc_param_map.value_max[i] = map_rc.param_value_max; + if (map_rc.param_index == -2) { // -2 means unset map + _rc_param_map.valid[i] = false; + } else { + _rc_param_map.valid[i] = true; + } + _rc_param_map.timestamp = hrt_absolute_time(); + + if (_rc_param_map_pub < 0) { + _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); + + } else { + orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); + } + + } + break; + } + default: break; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 5576e6b84..b6736f212 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -44,6 +44,8 @@ #include "mavlink_bridge_header.h" #include "mavlink_stream.h" +#include <uORB/uORB.h> +#include <uORB/topics/rc_parameter_map.h> class MavlinkParametersManager : public MavlinkStream { @@ -112,4 +114,7 @@ protected: void send(const hrt_abstime t); void send_param(param_t param); + + orb_advert_t _rc_param_map_pub; + struct rc_parameter_map_s _rc_param_map; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cb55a25aa..ce3f2ae0e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -68,6 +68,8 @@ #include <mathlib/mathlib.h> +#include <conversion/rotation.h> + #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> @@ -121,7 +123,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0f), - _hil_local_proj_ref{} + _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.6), + _time_offset(0) { // make sure the FTP server is started @@ -188,6 +192,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; + case MAVLINK_MSG_ID_SYSTEM_TIME: + handle_message_system_time(msg); + break; + + case MAVLINK_MSG_ID_TIMESYNC: + handle_message_timesync(msg); + break; + default: break; } @@ -251,7 +263,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -261,7 +273,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -307,7 +319,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -317,7 +329,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -358,6 +370,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); + struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -374,6 +389,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -689,7 +708,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time vision_position.timestamp_computer = pos.usec; vision_position.x = pos.x; vision_position.y = pos.y; @@ -861,7 +880,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); @@ -919,6 +938,66 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) +{ + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; + if(clock_settime(CLOCK_REALTIME, &tv)) { + warn("failed setting clock"); + } + else { + warnx("[timesync] UTC time synced."); + } + } + +} + +void +MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) +{ + mavlink_timesync_t tsync; + mavlink_msg_timesync_decode(msg, &tsync); + + uint64_t now_ns = hrt_absolute_time() * 1000LL ; + + if (tsync.tc1 == 0) { + + mavlink_timesync_t rsync; // return timestamped sync message + + rsync.tc1 = now_ns; + rsync.ts1 = tsync.ts1; + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); + + return; + + } else if (tsync.tc1 > 0) { + + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t dt = _time_offset - offset_ns; + + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); + } else { + smooth_time_offset(offset_ns); + } + } + +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; @@ -1110,7 +1189,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* print HIL sensors rate */ if ((timestamp - _old_timestamp) > 10000000) { - printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + // printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); _old_timestamp = timestamp; _hil_frames = 0; } @@ -1128,7 +1207,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) memset(&hil_gps, 0, sizeof(hil_gps)); hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; + hil_gps.time_utc_usec = gps.time_usec; hil_gps.timestamp_position = timestamp; hil_gps.lat = gps.lat; @@ -1385,6 +1464,23 @@ void MavlinkReceiver::print_status() } +uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +{ + return usec - (_time_offset / 1000) ; +} + + +void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + + void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a057074a7..4afc9b372 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -75,6 +75,8 @@ #include "mavlink_ftp.h" +#define PX4_EPOCH_SECS 1234567890ULL + class Mavlink; class MavlinkReceiver @@ -124,12 +126,23 @@ private: void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); + void handle_message_system_time(mavlink_message_t *msg); + void handle_message_timesync(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); void *receive_thread(void *arg); + /** + * Convert remote nsec timestamp to local hrt time (usec) + */ + uint64_t to_hrt(uint64_t nsec); + /** + * Exponential moving average filter to smooth time offset + */ + void smooth_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -164,6 +177,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; + uint64_t _time_offset; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver&); diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 1cc28cce1..b46d2bd35 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1f40e634e..5f8f8d02f 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,7 +50,8 @@ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters - * @min 1.0 + * @min 0.05 + * @max 200 * @group Mission */ -PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); /** * Set OBC mode for data link loss diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bfe6ce7e1..1568233b0 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -53,7 +53,8 @@ * Default value of loiter radius after RTL (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); @@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * * @unit meters * @min 0 - * @max 1 + * @max 150 * @group RTL */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); /** @@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); * Land (i.e. slowly descend) from this altitude if autolanding allowed. * * @unit meters - * @min 0 + * @min 2 * @max 100 * @group RTL */ @@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * * @unit seconds * @min -1 - * @max + * @max 300 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.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 6fb87ae7e..38011a935 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1167,7 +1167,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; + global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f1118000e..91a9611d4 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1200 +EXTRACFLAGS = -Wframe-larger-than=1300 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0a8564da6..abc69a4b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,12 +39,14 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Ban Siesta <bansiesta@gmail.com> */ #include <nuttx/config.h> #include <sys/types.h> #include <sys/stat.h> #include <sys/prctl.h> +#include <sys/statfs.h> #include <fcntl.h> #include <errno.h> #include <unistd.h> @@ -57,6 +59,7 @@ #include <unistd.h> #include <drivers/drv_hrt.h> #include <math.h> +#include <time.h> #include <drivers/drv_range_finder.h> @@ -103,6 +106,8 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +#define PX4_EPOCH_SECS 1234567890ULL + /** * Logging rate. * @@ -158,7 +163,9 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; -static const char *log_root = "/fs/microsd/log"; +#define MOUNTPOINT "/fs/microsd" +static const char *mountpoint = MOUNTPOINT; +static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -171,6 +178,7 @@ static char log_dir[32]; /* statistics counters */ static uint64_t start_time = 0; static unsigned long log_bytes_written = 0; +static unsigned long last_checked_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; @@ -185,6 +193,9 @@ static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; +/* flag if warning about MicroSD card being almost full has already been sent */ +static bool space_warning_sent = false; + static pthread_t logwriter_pthread = 0; static pthread_attr_t logwriter_attr; @@ -244,6 +255,11 @@ static bool file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); +/** + * Check if there is still free space available + */ +static int check_free_space(void); + static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); @@ -338,13 +354,16 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - if (log_name_timestamp && gps_time != 0) { - /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); - strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == OK) { @@ -384,8 +403,7 @@ int create_log_dir() } /* print logging path, important to find log file later */ - warnx("log dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } @@ -395,12 +413,16 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + /* start logging if we have a valid time and the time is not in the past */ + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -408,8 +430,8 @@ int open_log_file() /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + /* format log file path: e.g. /fs/microsd/sess001/log001.px4log */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); if (!file_exist(log_file_path)) { @@ -421,7 +443,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -429,12 +451,10 @@ int open_log_file() int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -446,12 +466,15 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); } else { @@ -459,7 +482,7 @@ int open_perf_file(const char* str) /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + /* format log file path: e.g. /fs/microsd/sess001/log001.txt */ snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); @@ -472,7 +495,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -480,12 +503,8 @@ int open_perf_file(const char* str) int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); - } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -547,6 +566,7 @@ static void *logwriter_thread(void *arg) pthread_mutex_unlock(&logbuffer_mutex); if (available > 0) { + /* do heavy IO here */ if (available > MAX_WRITE_CHUNK) { n = MAX_WRITE_CHUNK; @@ -584,6 +604,16 @@ static void *logwriter_thread(void *arg) if (++poll_count == 10) { fsync(log_fd); poll_count = 0; + + } + + if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) { + /* check if space is available, if not stop everything */ + if (check_free_space() != OK) { + logwriter_should_exit = true; + main_thread_should_exit = true; + } + last_checked_bytes_written = log_bytes_written; } } @@ -598,15 +628,15 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); - errx(1, "error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + exit(1); } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -644,8 +674,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -771,7 +800,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first"); + warnx("ERR: log stream, start mavlink app first"); } /* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */ @@ -899,11 +928,17 @@ int sdlog2_thread_main(int argc, char *argv[]) } + + if (check_free_space() != OK) { + errx(1, "ERR: MicroSD almost full"); + } + + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret != 0 && errno != EEXIST) { - err(1, "failed creating log root dir: %s", log_root); + err(1, "ERR: failed creating log dir: %s", log_root); } /* copy conversion scripts */ @@ -1112,6 +1147,7 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime barometer1_timestamp = 0; hrt_abstime gyro1_timestamp = 0; hrt_abstime accelerometer1_timestamp = 0; hrt_abstime magnetometer1_timestamp = 0; @@ -1127,7 +1163,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } } @@ -1155,7 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } if (!logging_enabled) { @@ -1185,7 +1221,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (gps_pos_updated) { log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; + log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; log_msg.body.log_GPS.eph = buf_gps_pos.eph; log_msg.body.log_GPS.epv = buf_gps_pos.epv; @@ -1257,6 +1293,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU1 = false; bool write_IMU2 = false; bool write_SENS = false; + bool write_SENS1 = false; if (buf.sensor.timestamp != gyro_timestamp) { gyro_timestamp = buf.sensor.timestamp; @@ -1307,6 +1344,22 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.baro1_timestamp != barometer1_timestamp) { + barometer1_timestamp = buf.sensor.baro1_timestamp; + write_SENS1 = true; + } + + if (write_SENS1) { + log_msg.msg_type = LOG_AIR1_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; + // XXX moving to AIR0-AIR2 instead of SENS + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; write_IMU1 = true; @@ -1737,8 +1790,6 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting"); - thread_running = false; return 0; @@ -1771,7 +1822,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy"); + warnx("ERR: open in"); return 1; } @@ -1779,7 +1830,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy"); + warnx("ERR: open out"); return 1; } @@ -1804,6 +1855,34 @@ int file_copy(const char *file_old, const char *file_new) return OK; } +int check_free_space() +{ + /* use statfs to determine the number of blocks left */ + FAR struct statfs statfs_buf; + if (statfs(mountpoint, &statfs_buf) != OK) { + errx(ERROR, "ERR: statfs"); + } + + /* use a threshold of 50 MiB */ + if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] no space on MicroSD: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); + /* we do not need a flag to remember that we sent this warning because we will exit anyway */ + return ERROR; + + /* use a threshold of 100 MiB to send a warning */ + } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] space on MicroSD low: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); + /* we don't want to flood the user with warnings */ + space_warning_sent = true; + } + + return OK; +} + void handle_command(struct vehicle_command_s *cmd) { int param; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5b047f538..5941bfac0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -424,6 +424,9 @@ struct log_ENCD_s { float vel1; }; +/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ +#define LOG_AIR1_MSG 40 + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -457,7 +460,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), 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, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce..a065541b9 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -262,6 +262,25 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * PX4Flow board rotation + * + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Zero rotation is defined as Y on flow board pointing towards front of vehicle + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); + +/** * Board rotation Y (Pitch) offset * * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user @@ -747,6 +766,41 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fca72c304..1ca9ecd8f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -37,7 +37,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> @@ -63,6 +63,7 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_adc.h> #include <drivers/drv_airspeed.h> +#include <drivers/drv_px4flow.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -82,6 +83,7 @@ #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/airspeed.h> +#include <uORB/topics/rc_parameter_map.h> #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -191,6 +193,14 @@ private: uint8_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** + * Update paramters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + + /** * Gather and publish RC input data. */ void rc_poll(); @@ -216,10 +226,12 @@ private: int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ + int _baro1_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ @@ -237,6 +249,7 @@ private: struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + struct rc_parameter_map_s _rc_parameter_map; math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ @@ -263,6 +276,7 @@ private: float diff_pres_analog_scale; int board_rotation; + int flow_rotation; int external_mag_rotation; float board_offset[3]; @@ -288,6 +302,8 @@ private: int rc_map_aux4; int rc_map_aux5; + int rc_map_param[RC_PARAM_MAP_NCHAN]; + int32_t rc_fails_thr; float rc_assist_th; float rc_auto_th; @@ -348,6 +364,12 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; + param_t rc_map_param[RC_PARAM_MAP_NCHAN]; + param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + param_t rc_fails_thr; param_t rc_assist_th; param_t rc_auto_th; @@ -361,6 +383,7 @@ private: param_t battery_current_scaling; param_t board_rotation; + param_t flow_rotation; param_t external_mag_rotation; param_t board_offset[3]; @@ -378,27 +401,27 @@ private: /** * Do accel-related initialisation. */ - void accel_init(); + int accel_init(); /** * Do gyro-related initialisation. */ - void gyro_init(); + int gyro_init(); /** * Do mag-related initialisation. */ - void mag_init(); + int mag_init(); /** * Do baro-related initialisation. */ - void baro_init(); + int baro_init(); /** * Do adc-related initialisation. */ - void adc_init(); + int adc_init(); /** * Poll the accelerometer for updated data. @@ -451,6 +474,11 @@ private: void parameter_update_poll(bool forced = false); /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + + /** * Poll the ADC and update readings to suit. * * @param raw Combined sensor data structure into which @@ -479,7 +507,7 @@ Sensors::Sensors() : _fd_adc(-1), _last_adc(0), - _task_should_exit(false), + _task_should_exit(true), _sensors_task(-1), _hil_enabled(false), _publishing(true), @@ -496,8 +524,10 @@ Sensors::Sensors() : _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), + _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), + _rc_parameter_map_sub(-1), _manual_control_sub(-1), /* publications */ @@ -518,6 +548,7 @@ Sensors::Sensors() : { memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -570,6 +601,13 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + char name[PARAM_ID_LEN]; + snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + _parameter_handles.rc_map_param[i] = param_find(name); + } + /* RC thresholds */ _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); @@ -614,6 +652,7 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ @@ -747,6 +786,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -791,6 +833,10 @@ Sensors::parameters_update() _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } + /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); @@ -831,8 +877,22 @@ Sensors::parameters_update() } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + /* set px4flow rotation */ + int flowfd; + flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + if (flowret) { + warnx("flow rotation could not be set"); + close(flowfd); + return ERROR; + } + close(flowfd); + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -850,26 +910,26 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int fd; - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: no barometer found"); + int barofd; + barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { + warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); + return ERROR; } else { - int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - if (ret) { + int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); - close(fd); + close(barofd); return ERROR; } - close(fd); + close(barofd); } return OK; } -void +int Sensors::accel_init() { int fd; @@ -877,8 +937,8 @@ Sensors::accel_init() fd = open(ACCEL_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); - errx(1, "FATAL: no accelerometer found"); + warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH); + return ERROR; } else { @@ -907,9 +967,11 @@ Sensors::accel_init() close(fd); } + + return OK; } -void +int Sensors::gyro_init() { int fd; @@ -917,8 +979,8 @@ Sensors::gyro_init() fd = open(GYRO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); - errx(1, "FATAL: no gyro found"); + warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH); + return ERROR; } else { @@ -948,9 +1010,11 @@ Sensors::gyro_init() close(fd); } + + return OK; } -void +int Sensors::mag_init() { int fd; @@ -959,8 +1023,8 @@ Sensors::mag_init() fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); - errx(1, "FATAL: no magnetometer found"); + warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH); + return ERROR; } /* try different mag sampling rates */ @@ -981,7 +1045,8 @@ Sensors::mag_init() ioctl(fd, SENSORIOCSPOLLRATE, 100); } else { - errx(1, "FATAL: mag sampling rate could not be set"); + warnx("FATAL: mag sampling rate could not be set"); + return ERROR; } } @@ -990,7 +1055,8 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) { - errx(1, "FATAL: unknown if magnetometer is external or onboard"); + warnx("FATAL: unknown if magnetometer is external or onboard"); + return ERROR; } else if (ret == 1) { _mag_is_external = true; @@ -1000,9 +1066,11 @@ Sensors::mag_init() } close(fd); + + return OK; } -void +int Sensors::baro_init() { int fd; @@ -1010,26 +1078,30 @@ Sensors::baro_init() fd = open(BARO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: No barometer found"); + warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH); + return ERROR; } /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); close(fd); + + return OK; } -void +int Sensors::adc_init() { _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { - warn(ADC_DEVICE_PATH); - warnx("FATAL: no ADC found"); + warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH); + return ERROR; } + + return OK; } void @@ -1200,6 +1272,34 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; } + + orb_check(_mag1_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + + raw.magnetometer1_raw[0] = mag_report.x_raw; + raw.magnetometer1_raw[1] = mag_report.y_raw; + raw.magnetometer1_raw[2] = mag_report.z_raw; + + raw.magnetometer1_timestamp = mag_report.timestamp; + } + + orb_check(_mag2_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + + raw.magnetometer2_raw[0] = mag_report.x_raw; + raw.magnetometer2_raw[1] = mag_report.y_raw; + raw.magnetometer2_raw[2] = mag_report.z_raw; + + raw.magnetometer2_timestamp = mag_report.timestamp; + } } void @@ -1218,6 +1318,21 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_timestamp = _barometer.timestamp; } + + orb_check(_baro1_sub, &baro_updated); + + if (baro_updated) { + + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + + raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro1_timestamp = baro_report.timestamp; + } } void @@ -1374,6 +1489,45 @@ Sensors::parameter_update_poll(bool forced) } void +Sensors::rc_parameter_map_poll(bool forced) +{ + bool map_updated; + orb_check(_rc_parameter_map_sub, &map_updated); + + if (map_updated) { + orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { + _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + } + + } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); + } + } +} + +void Sensors::adc_poll(struct sensor_combined_s &raw) { /* only read if publishing */ @@ -1552,6 +1706,31 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) } void +Sensors::set_params_from_rc() +{ + static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { + param_rc_values[i] = rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + +void Sensors::rc_poll() { bool rc_updated; @@ -1717,6 +1896,13 @@ Sensors::rc_poll() } else { _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } + + /* Update parameters from RC Channels (tuning with RC) if activated */ + static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { + set_params_from_rc(); + last_rc_to_param_map_time = hrt_absolute_time(); + } } } } @@ -1732,11 +1918,27 @@ Sensors::task_main() { /* start individual sensors */ - accel_init(); - gyro_init(); - mag_init(); - baro_init(); - adc_init(); + int ret; + ret = accel_init(); + if (ret) { + goto exit_immediate; + } + ret = gyro_init(); + if (ret) { + goto exit_immediate; + } + ret = mag_init(); + if (ret) { + goto exit_immediate; + } + ret = baro_init(); + if (ret) { + goto exit_immediate; + } + ret = adc_init(); + if (ret) { + goto exit_immediate; + } /* * do subscriptions @@ -1752,9 +1954,11 @@ Sensors::task_main() _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ @@ -1788,6 +1992,7 @@ Sensors::task_main() diff_pres_poll(raw); parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); @@ -1799,6 +2004,8 @@ Sensors::task_main() fds[0].fd = _gyro_sub; fds[0].events = POLLIN; + _task_should_exit = false; + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -1820,6 +2027,9 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ @@ -1846,8 +2056,9 @@ Sensors::task_main() warnx("[sensors] exiting."); +exit_immediate: _sensors_task = -1; - _exit(0); + _exit(ret); } int @@ -1863,9 +2074,13 @@ Sensors::start() (main_t)&Sensors::task_main_trampoline, nullptr); + /* wait until the task is up and running or has failed */ + while(_sensors_task > 0 && _task_should_exit) { + usleep(100); + } + if (_sensors_task < 0) { - warn("task start failed"); - return -errno; + return -ERROR; } return OK; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 24187c9bc..eb1aef6c1 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -78,89 +78,87 @@ float constrain(float val, float min, float max) */ const MultirotorMixer::Rotor _config_quad_x[] = { - { -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.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_plus[] = { - { -1.000000, 0.000000, 1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, + { -1.000000, 0.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, }; -//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.3223, 0.9466, 0.4242 }, - { 0.3223, -0.9466, 1.0000 }, - { 0.3223, 0.9466, -0.4242 }, - { -0.3223, -0.9466, -1.0000 }, + { -0.322266, 0.946649, 0.424200 }, + { 0.322266, 0.946649, 1.000000 }, + { 0.322266, 0.946649, -0.424200 }, + { -0.322266, 0.946649, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.777146, -0.629320, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.777146, -0.629320, -1.00 }, + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { - { -1.000000, 0.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.500000, 0.866025, -1.00 }, - { -0.500000, -0.866025, 1.00 }, - { -0.500000, 0.866025, 1.00 }, - { 0.500000, -0.866025, -1.00 }, + { -1.000000, 0.000000, -1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.500000, 0.866025, -1.000000 }, + { -0.500000, -0.866025, 1.000000 }, + { -0.500000, 0.866025, 1.000000 }, + { 0.500000, -0.866025, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, -0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { 0.866025, 0.500000, 1.00 }, - { -0.866025, -0.500000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, -0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { 0.866025, 0.500000, 1.000000 }, + { -0.866025, -0.500000, -1.000000 }, }; 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 }, + { -0.866025, 0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, 0.500000, -1.000000 }, + { 0.866025, 0.500000, 1.000000 }, }; const MultirotorMixer::Rotor _config_octa_x[] = { - { -0.382683, 0.923880, -1.00 }, - { 0.382683, -0.923880, -1.00 }, - { -0.923880, 0.382683, 1.00 }, - { -0.382683, -0.923880, 1.00 }, - { 0.382683, 0.923880, 1.00 }, - { 0.923880, -0.382683, 1.00 }, - { 0.923880, 0.382683, -1.00 }, - { -0.923880, -0.382683, -1.00 }, + { -0.382683, 0.923880, -1.000000 }, + { 0.382683, -0.923880, -1.000000 }, + { -0.923880, 0.382683, 1.000000 }, + { -0.382683, -0.923880, 1.000000 }, + { 0.382683, 0.923880, 1.000000 }, + { 0.923880, -0.382683, 1.000000 }, + { 0.923880, 0.382683, -1.000000 }, + { -0.923880, -0.382683, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -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 }, - { 1.000000, 0.000000, -1.00 }, - { -1.000000, 0.000000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.707107, 0.707107, 1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 1.000000, 0.000000, -1.000000 }, + { -1.000000, 0.000000, -1.000000 }, }; 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 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { -0.707107, -0.707107, -1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { -0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, -0.707107, -1.000000 }, }; -const MultirotorMixer::Rotor _config_duorotor[] = { - { -1.000000, 0.000000, 0.00 }, - { 1.000000, 0.000000, 0.00 }, +const MultirotorMixer::Rotor _config_twin_engine[] = { + { -1.000000, 0.000000, 0.000000 }, + { 1.000000, 0.000000, 0.000000 }, }; - const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], - &_config_duorotor[0], + &_config_twin_engine[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 18c828578..bdb62f812 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -21,6 +21,12 @@ set quad_plus { 180 CW } +set quad_v { + 18.8 0.4242 + -18.8 1.0 + -18.8 -0.4242 + 18.8 -1.0 +} set quad_wide { 68 CCW @@ -89,11 +95,14 @@ set octa_cox { -135 CW } +set twin_engine { + 90 0.0 + -90 0.0 +} -set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox 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 twin_engine} -proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} foreach table $tables { puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] @@ -101,9 +110,11 @@ foreach table $tables { upvar #0 $table angles foreach {angle dir} $angles { if {$dir == "CW"} { + set dd -1.0 + } elseif {$dir == "CCW"} { set dd 1.0 } else { - set dd -1.0 + set dd $dir } factors $angle $dd } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3d5755a46..293412455 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -63,6 +63,7 @@ ORB_DEFINE(sensor_gyro2, struct gyro_report); #include <drivers/drv_baro.h> ORB_DEFINE(sensor_baro0, struct baro_report); ORB_DEFINE(sensor_baro1, struct baro_report); +ORB_DEFINE(sensor_baro2, struct baro_report); #include <drivers/drv_range_finder.h> ORB_DEFINE(sensor_range_finder, struct range_finder_report); @@ -246,3 +247,6 @@ ORB_DEFINE(tecs_status, struct tecs_status_s); #include "topics/wind_estimate.h" ORB_DEFINE(wind_estimate, struct wind_estimate_s); + +#include "topics/rc_parameter_map.h" +ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h new file mode 100644 index 000000000..6e68dc4b6 --- /dev/null +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT , + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_parameter_map.h + * Maps RC channels to parameters + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef TOPIC_RC_PARAMETER_MAP_H +#define TOPIC_RC_PARAMETER_MAP_H + +#include <stdint.h> +#include "../uORB.h" + +#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +/** + * @addtogroup topics + * @{ + */ + +struct rc_parameter_map_s { + uint64_t timestamp; /**< time at which the map was updated */ + + bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ + + int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this + this field is ignored if set to -1, in this case param_id will + be used*/ + char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ + float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ + float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ + float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ + float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ +}; + +/** + * @} + */ + +ORB_DECLARE(rc_parameter_map); + +#endif diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 06dfcdab3..583a39ded 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -122,15 +122,25 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + uint64_t baro_timestamp; /**< Barometer timestamp */ + + float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ + float baro1_alt_meter; /**< Altitude, already temp. comp. */ + float baro1_temp_celcius; /**< Temperature in degrees celsius */ + uint64_t baro1_timestamp; /**< Barometer timestamp */ + + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ + float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f7a432495..137c86dd5 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,7 +62,7 @@ */ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 7888a50f4..102914bbb 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -79,7 +79,7 @@ struct vehicle_gps_position_s { bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ + uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ uint8_t satellites_used; /**< Number of satellites used */ }; diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 24ecca9fa..7b4e22bc8 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -54,6 +54,7 @@ struct vtol_vehicle_status_s { uint64_t timestamp; /**< Microseconds since system boot */ bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ + bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ }; /** diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index a375db37f..571a6f1cd 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -159,7 +159,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca report.vel_ned_valid = true; report.timestamp_time = report.timestamp_position; - report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds report.satellites_used = msg.sats_used; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 9a80562f3..c72a85ecd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -134,17 +134,21 @@ private: struct { param_t idle_pwm_mc; //pwm value for idle in mc mode param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode + float fw_pitch_trim; // trim for neutral elevon position in fw mode } _params; struct { param_t idle_pwm_mc; param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; param_t mc_airspeed_min; param_t mc_airspeed_trim; param_t mc_airspeed_max; + param_t fw_pitch_trim; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -234,12 +238,15 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; + _params.vtol_fw_permanent_stab = 0; _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); + _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); /* fetch initial parameter values */ parameters_update(); @@ -411,6 +418,9 @@ VtolAttitudeControl::parameters_update() /* vtol motor count */ param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); + /* vtol fw permanent stabilization */ + param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab); + /* vtol mc mode min airspeed */ param_get(_params_handles.mc_airspeed_min, &v); _params.mc_airspeed_min = v; @@ -423,6 +433,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.mc_airspeed_trim, &v); _params.mc_airspeed_trim = v; + /* vtol pitch trim for fw mode */ + param_get(_params_handles.fw_pitch_trim, &v); + _params.fw_pitch_trim = v; + return OK; } @@ -452,7 +466,7 @@ void VtolAttitudeControl::fill_fw_att_control_output() _actuators_out_0.control[3] = _actuators_fw_in.control[3]; /*controls for the elevons */ _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon + _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon // unused now but still logged _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle @@ -597,6 +611,9 @@ void VtolAttitudeControl::task_main() parameters_update(); // initialize parameter cache + /* update vtol vehicle status*/ + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + // make sure we start with idle in mc mode set_idle_mc(); flag_idle_mc = true; @@ -647,6 +664,8 @@ void VtolAttitudeControl::task_main() parameters_update(); } + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index e21bccb0c..d1d4697f3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -86,3 +86,26 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); */ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +/** + * Permanent stabilization in fw mode + * + * If set to one this parameter will cause permanent attitude stabilization in fw mode. + * This parameter has been introduced for pure convenience sake. + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); + +/** + * Fixed wing pitch trim + * + * This parameter allows to adjust the neutral elevon position in fixed wing mode. + * + * @min -1 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); + |