diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-13 20:43:22 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-13 20:43:22 +0200 |
commit | e412bce1a16d0c9a6e3ae3f51bdbe4de8be527fa (patch) | |
tree | c0d77e6ee13aa09d10a0a278fa086cc35450a64d /src | |
parent | bd9d58f565383598db785908a7cc08f87f6a07f1 (diff) | |
parent | 178a3e8567b3e721771fffcb8f32df140ad1038b (diff) | |
download | px4-firmware-e412bce1a16d0c9a6e3ae3f51bdbe4de8be527fa.tar.gz px4-firmware-e412bce1a16d0c9a6e3ae3f51bdbe4de8be527fa.tar.bz2 px4-firmware-e412bce1a16d0c9a6e3ae3f51bdbe4de8be527fa.zip |
Merge branch 'master' into ekf_auto_mag_decl
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/blinkm/blinkm.cpp | 67 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 10 | ||||
-rw-r--r-- | src/drivers/hott/messages.cpp | 32 | ||||
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 36 | ||||
-rw-r--r-- | src/drivers/stm32/drv_hrt.c | 8 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 4 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.cpp | 6 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 23 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 20 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 8 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 10 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 8 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 16 | ||||
-rw-r--r-- | src/modules/systemlib/rc_check.c | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/manual_control_setpoint.h | 35 |
18 files changed, 173 insertions, 132 deletions
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index c41d8f242..5c502f682 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -194,6 +194,26 @@ private: bool systemstate_run; + int vehicle_status_sub_fd; + int vehicle_control_mode_sub_fd; + int vehicle_gps_position_sub_fd; + int actuator_armed_sub_fd; + int safety_sub_fd; + + int num_of_cells; + int detected_cells_runcount; + + int t_led_color[8]; + int t_led_blink; + int led_thread_runcount; + int led_interval; + + bool topic_initialized; + bool detected_cells_blinked; + bool led_thread_ready; + + int num_of_used_sats; + void setLEDColor(int ledcolor); static void led_trampoline(void *arg); void led(); @@ -265,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) : led_color_7(LED_OFF), led_color_8(LED_OFF), led_blink(LED_NOBLINK), - systemstate_run(false) + systemstate_run(false), + vehicle_status_sub_fd(-1), + vehicle_control_mode_sub_fd(-1), + vehicle_gps_position_sub_fd(-1), + actuator_armed_sub_fd(-1), + safety_sub_fd(-1), + num_of_cells(0), + detected_cells_runcount(0), + t_led_color({0}), + t_led_blink(0), + led_thread_runcount(0), + led_interval(1000), + topic_initialized(false), + detected_cells_blinked(false), + led_thread_ready(true), + num_of_used_sats(0) { memset(&_work, 0, sizeof(_work)); } @@ -382,31 +417,6 @@ void BlinkM::led() { - static int vehicle_status_sub_fd; - static int vehicle_control_mode_sub_fd; - static int vehicle_gps_position_sub_fd; - static int actuator_armed_sub_fd; - static int safety_sub_fd; - - static int num_of_cells = 0; - static int detected_cells_runcount = 0; - - static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; - static int t_led_blink = 0; - static int led_thread_runcount=0; - static int led_interval = 1000; - - static int no_data_vehicle_status = 0; - static int no_data_vehicle_control_mode = 0; - static int no_data_actuator_armed = 0; - static int no_data_vehicle_gps_position = 0; - - static bool topic_initialized = false; - static bool detected_cells_blinked = false; - static bool led_thread_ready = true; - - int num_of_used_sats = 0; - if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 250); @@ -494,6 +504,11 @@ BlinkM::led() orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); + int no_data_vehicle_status = 0; + int no_data_vehicle_control_mode = 0; + int no_data_actuator_armed = 0; + int no_data_vehicle_gps_position = 0; + if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cdc9d3106..fddba806e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -158,6 +158,7 @@ private: int _class_instance; orb_advert_t _mag_topic; + orb_advert_t _subsystem_pub; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -324,7 +325,9 @@ HMC5883::HMC5883(int bus) : _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), + _collect_phase(false), _mag_topic(-1), + _subsystem_pub(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), @@ -1137,13 +1140,12 @@ int HMC5883::check_calibration() true, _calibrated, SUBSYSTEM_TYPE_MAG}; - static orb_advert_t pub = -1; if (!(_pub_blocked)) { - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (_subsystem_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); } } } diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 90a744015..1e779e8dc 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -51,6 +51,8 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> +#include <drivers/drv_hrt.h> + /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 @@ -62,7 +64,6 @@ static int _airspeed_sub = -1; static int _esc_sub = -1; static orb_advert_t _esc_pub; -struct esc_status_s _esc; static bool _home_position_set = false; static double _home_lat = 0.0d; @@ -82,8 +83,6 @@ init_sub_messages(void) void init_pub_messages(void) { - memset(&_esc, 0, sizeof(_esc)); - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); } void @@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer) size_t size = sizeof(msg); memset(&msg, 0, size); memcpy(&msg, buffer, size); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + + // Publish it. + esc.timestamp = hrt_absolute_time(); + esc.esc_count = 1; + esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + esc.esc[0].esc_temperature = msg.temperature1 - 20; + esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { - orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + orb_publish(ORB_ID(esc_status), _esc_pub, &esc); } else { - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } - - // Publish it. - _esc.esc_count = 1; - _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; - - _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; - _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - _esc.esc[0].esc_temperature = msg.temperature1 - 20; - _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); } void diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 0915c122b..5954c40da 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -92,8 +92,20 @@ #define MOTOR_SPINUP_COUNTER 30 #define ESC_UORB_PUBLISH_DELAY 500000 - - +struct MotorData_t { + unsigned int Version; // the version of the BL (0 = old) + unsigned int SetPoint; // written by attitude controller + unsigned int SetPointLowerBits; // for higher Resolution of new BLs + float SetPoint_PX4; // Values from PX4 + unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present + unsigned int ReadMode; // select data to read + unsigned short RawPwmValue; // length of PWM pulse + // the following bytes must be exactly in that order! + unsigned int Current; // in 0.1 A steps, read back from BL + unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit + unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in + unsigned int RoundCount; +}; class MK : public device::I2C { @@ -154,6 +166,8 @@ private: actuator_controls_s _controls; + MotorData_t Motor[MAX_MOTORS]; + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -195,24 +209,6 @@ const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0}; -struct MotorData_t { - unsigned int Version; // the version of the BL (0 = old) - unsigned int SetPoint; // written by attitude controller - unsigned int SetPointLowerBits; // for higher Resolution of new BLs - float SetPoint_PX4; // Values from PX4 - unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present - unsigned int ReadMode; // select data to read - unsigned short RawPwmValue; // length of PWM pulse - // the following bytes must be exactly in that order! - unsigned int Current; // in 0.1 A steps, read back from BL - unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit - unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in - unsigned int RoundCount; -}; - -MotorData_t Motor[MAX_MOTORS]; - - namespace { diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index f324b341e..5bb550279 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; +#define PPM_DEBUG 0 + +#if PPM_DEBUG /* PPM edge history */ __EXPORT uint16_t ppm_edge_history[32]; unsigned ppm_edge_next; @@ -361,6 +364,7 @@ unsigned ppm_edge_next; /* PPM pulse history */ __EXPORT uint16_t ppm_pulse_history[32]; unsigned ppm_pulse_next; +#endif static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; @@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status) /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; +#if PPM_DEBUG ppm_edge_history[ppm_edge_next++] = width; if (ppm_edge_next >= 32) ppm_edge_next = 0; +#endif /* * if this looks like a start pulse, then push the last set of values @@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status) interval = count - ppm.last_mark; ppm.last_mark = count; +#if PPM_DEBUG ppm_pulse_history[ppm_pulse_next++] = interval; if (ppm_pulse_next >= 32) ppm_pulse_next = 0; +#endif /* if the mark-mark timing is out of bounds, abandon the frame */ if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 22504642f..17d3d3dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1188,7 +1188,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1206,7 +1206,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ 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 a40c402d6..1c411fa06 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -706,20 +706,21 @@ FixedwingAttitudeControl::task_main() } else { /* * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. + * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -825,10 +826,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.roll; - _actuators.control[1] = -_manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; + _actuators.control[0] = _manual.y; + _actuators.control[1] = -_manual.x; + _actuators.control[2] = _manual.r; + _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } 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 c62a2d4f3..9cbc26efe 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 @@ -1114,7 +1114,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.yaw; + _altctrl_hold_heading = _att.yaw + _manual.r; } //XXX not used @@ -1132,12 +1132,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, @@ -1153,7 +1153,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.yaw; + _altctrl_hold_heading = _att.yaw + _manual.r; } /* if in altctrl mode, set airspeed based on manual control */ @@ -1161,10 +1161,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; /* user switched off throttle */ - if (_manual.throttle < 0.1f) { + if (_manual.z < 0.1f) { throttle_max = 0.0f; /* switch to pure pitch based altitude control, give up speed */ _tecs.set_speed_weight(0.0f); @@ -1174,14 +1174,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climb_out = false; /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _manual.roll; - _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + _att_sp.roll_body = _manual.y; + _att_sp.yaw_body = _manual.r; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 833de5a3d..dd88b0949 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -727,9 +727,9 @@ int Mavlink::mavlink_pm_send_param(param_t param) if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; float val_buf; - static mavlink_message_t tx_msg; + mavlink_message_t tx_msg; /* query parameter type */ param_type_t type = param_type(param); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b8879157e..79dd88657 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1138,10 +1138,10 @@ protected: if (manual_sub->update(t)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, + manual->x * 1000, + manual->y * 1000, + manual->z * 1000, + manual->r * 1000, 0); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64fc41838..b03a68c07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); @@ -438,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) memset(&manual, 0, sizeof(manual)); manual.timestamp = hrt_absolute_time(); - manual.pitch = man.x / 1000.0f; - manual.roll = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 57450cf39..a69153bf0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -486,7 +486,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.throttle; + _v_att_sp.thrust = _manual_control_sp.z; publish_att_sp = true; } @@ -504,7 +504,7 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max; + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -520,8 +520,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 45ff8c3c0..6e611d4ab 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -617,7 +617,7 @@ MulticopterPositionControl::task_main() reset_alt_sp(); /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { @@ -625,8 +625,8 @@ MulticopterPositionControl::task_main() reset_pos_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.pitch; - sp_move_rate(1) = _manual.roll; + sp_move_rate(0) = _manual.x; + sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ @@ -782,7 +782,7 @@ MulticopterPositionControl::task_main() float i = _params.thr_min; if (reset_int_z_manual) { - i = _manual.throttle; + i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..646ab6ab9 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -100,8 +100,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -125,8 +125,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 65165bbb2..18bf97f8d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1486,10 +1486,10 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.roll = get_rc_value(ROLL, -1.0, 1.0); - manual.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual.yaw = get_rc_value(YAW, -1.0, 1.0); - manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual.y = get_rc_value(ROLL, -1.0, 1.0); + manual.x = get_rc_value(PITCH, -1.0, 1.0); + manual.r = get_rc_value(YAW, -1.0, 1.0); + manual.z = get_rc_value(THROTTLE, 0.0, 1.0); manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); @@ -1517,10 +1517,10 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_last_signal; - actuator_group_3.control[0] = manual.roll; - actuator_group_3.control[1] = manual.pitch; - actuator_group_3.control[2] = manual.yaw; - actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; actuator_group_3.control[4] = manual.flaps; actuator_group_3.control[5] = manual.aux1; actuator_group_3.control[6] = manual.aux2; diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 21e15ec56..c0c1a5cb4 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index ee159b998..19a29635b 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -64,17 +64,34 @@ struct manual_control_setpoint_s { /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. + * The variable names follow the definition of the + * MANUAL_CONTROL mavlink message. + * The default range is from -1 to 1 (mavlink message -1000 to 1000) + * The range for the z variable is defined from 0 to 1. (The z field of + * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ - float roll; /**< ailerons roll / roll rate input, -1..1 */ - float pitch; /**< elevator / pitch / pitch rate, -1..1 */ - float yaw; /**< rudder / yaw rate / yaw, -1..1 */ - float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float x; /**< stick position in x direction -1..1 + in general corresponds to forward/back motion or pitch of vehicle, + in general a positive value means forward or negative pitch and + a negative value means backward or positive pitch */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle, + in general a positive value means right or positive roll and + a negative value means left or negative roll */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle, + in general the value corresponds to the demanded throttle by the user, + if the input is used for setting the setpoint of a vertical position + controller any value > 0.5 means up and any value < 0.5 means down */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to the righthand rotation around the vertical + (downwards) axis of the vehicle */ float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ |