diff options
Diffstat (limited to 'src')
24 files changed, 133 insertions, 86 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 52a667403..ec9d4ca09 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** stop DSM bind */ -#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) - /** Power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae5d6ce19..ed98ff0a5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -195,6 +195,12 @@ private: bool _dsm_vcc_ctl; /** + * System armed + */ + + bool _system_armed; + + /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); @@ -363,7 +369,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _dsm_vcc_ctl(false) + _dsm_vcc_ctl(false), + _system_armed(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -580,9 +587,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -690,6 +699,25 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; + int32_t dsm_bind_val; + param_t dsm_bind_param; + + // See if bind parameter has been set, and reset it to 0 + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!_system_armed) { + if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + dsm_bind_val = 0; + param_set(dsm_bind_param, &dsm_bind_val); + } /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -756,6 +784,8 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; + _system_armed = vstatus.flag_system_armed; + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { @@ -1452,16 +1482,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - usleep(1000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(100000); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - break; - - case DSM_BIND_STOP: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); - usleep(500000); break; case DSM_BIND_POWER_UP: @@ -1736,30 +1761,12 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - errx(1, "failed opening console"); - warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - warnx("Press CTRL-C or 'c' when done."); g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - for (;;) { - usleep(500000L); - /* Check if user wants to quit */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - warnx("Done\n"); - g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); - g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); - close(console); - exit(0); - } - } - } + exit(0); + } void diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 74037d1c2..796c6cd9f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -300,6 +300,8 @@ controls_tick() { } else { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 598bcee34..4925ced54 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -129,9 +129,9 @@ dsm_bind(uint16_t cmd, int pulses) case dsm_bind_send_pulses: for (int i = 0; i < pulses; i++) { stm32_gpiowrite(usart1RxAsOutp, false); - up_udelay(50); + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(50); + up_udelay(25); } break; case dsm_bind_reinit_uart: diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index ba4d2186c..255dfed18 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f7b41b120..42268b971 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -257,8 +257,6 @@ private: float battery_voltage_scaling; - int rc_rl1_DSM_VCC_control; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -308,8 +306,6 @@ private: param_t battery_voltage_scaling; - param_t rc_rl1_DSM_VCC_control; - } _parameter_handles; /**< handles for interesting parameters */ @@ -544,9 +540,6 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); - /* DSM VCC relay control */ - _parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC"); - /* fetch initial parameter values */ parameters_update(); } @@ -738,11 +731,6 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } - /* relay 1 DSM VCC control */ - if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) { - warnx("Failed updating relay 1 DSM VCC control"); - } - return OK; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61..301cfa255 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -37,6 +37,10 @@ * Common object definitions without a better home. */ +/** + * @defgroup topics List of all uORB topics. + */ + #include <nuttx/config.h> #include <drivers/drv_orb_dev.h> diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..a27095be5 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,11 +52,20 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index 088c4fc8f..d7b404ad4 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -53,11 +53,20 @@ #define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_effective_s { uint64_t timestamp; float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_effective_0); ORB_DECLARE(actuator_controls_effective_1); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index bbe429073..30895ca83 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -52,12 +52,21 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ int noutputs; /**< valid outputs */ }; +/** + * @} + */ + /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs_0); ORB_DECLARE(actuator_outputs_1); diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h index a9d1b83fd..9253c787d 100644 --- a/src/modules/uORB/topics/debug_key_value.h +++ b/src/modules/uORB/topics/debug_key_value.h @@ -47,6 +47,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 00cf59b28..11332d7a7 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -52,10 +52,6 @@ #include "../uORB.h" /** - * @addtogroup topics @{ - */ - -/** * The number of ESCs supported. * Current (Q2/2013) we support 8 ESCs, */ @@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE { }; /** - * + * @addtogroup topics + * @{ + */ + +/** + * Electronic speed controller status. */ struct esc_status_s { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 253f444b3..978a3383a 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum NAV_CMD { NAV_CMD_WAYPOINT = 0, NAV_CMD_LOITER_TURN_COUNT, @@ -62,6 +57,11 @@ enum NAV_CMD { }; /** + * @addtogroup topics + * @{ + */ + +/** * Global position setpoint in WGS84 coordinates. * * This is the position the MAV is heading towards. If it of type loiter, diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2e895c59c..7901b930a 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -44,11 +44,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Off-board control inputs. * * Typically sent by a ground control station / joystick or by @@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; +/** + * @addtogroup topics + * @{ + */ + struct offboard_control_setpoint_s { uint64_t timestamp; diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h index 8f4be3b3f..a6ad8a131 100644 --- a/src/modules/uORB/topics/omnidirectional_flow.h +++ b/src/modules/uORB/topics/omnidirectional_flow.h @@ -46,6 +46,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 300e895c7..68964deb0 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -42,11 +42,20 @@ #include <stdint.h> #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + struct parameter_update_s { /** time at which the latest parameter was updated */ uint64_t timestamp; }; +/** + * @} + */ + ORB_DECLARE(parameter_update); #endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 9dd54df91..e69335b3d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, * leaving at a sane value of 14. @@ -83,6 +78,11 @@ enum RC_CHANNELS_FUNCTION RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; +/** + * @addtogroup topics + * @{ + */ + struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 9a76b5182..ad164555e 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum MAGNETOMETER_MODE { MAGNETOMETER_MODE_NORMAL = 0, MAGNETOMETER_MODE_POSITIVE_BIAS, @@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE { }; /** + * @addtogroup topics + * @{ + */ + +/** * Sensor readings in raw and SI-unit form. * * These values are read from the sensors. Raw values are in sensor-specific units, diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index c415e832e..cfe0bf69e 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,10 +50,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - */ - enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, @@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE }; /** + * @addtogroup topics + */ + +/** * State of individual sub systems */ struct subsystem_info_s { diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index f30852de5..828fb31cc 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE { TELEMETRY_STATUS_RADIO_TYPE_WIRE }; +/** + * @addtogroup topics + * @{ + */ + struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ @@ -62,6 +67,10 @@ struct telemetry_status_s { uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; +/** + * @} + */ + ORB_DECLARE(telemetry_status); #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index c31c81d0c..4380a5ee7 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -48,6 +48,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index fac571659..31ff014de 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Commands for commander app. * * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, @@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ }; +/** + * @addtogroup topics + * @{ + */ struct vehicle_command_s { diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 318abba89..8516b263f 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -60,8 +60,8 @@ */ struct vehicle_global_position_set_triplet_s { - bool previous_valid; - bool next_valid; + bool previous_valid; /**< flag indicating previous position is valid */ + bool next_valid; /**< flag indicating next position is valid */ struct vehicle_global_position_setpoint_s previous; struct vehicle_global_position_setpoint_s current; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f6..94068a9ac 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,10 +54,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics @{ - */ - /* State Machine */ typedef enum { @@ -137,6 +133,10 @@ enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ }; +/** + * @addtogroup topics + * @{ + */ /** * state machine / state of vehicle. |