From e421260f7c3e2a3f7145c2f643f8440060a84777 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:57:53 +0100 Subject: Removed bogus sensor counters, replaced them with proper timestamps --- src/modules/uORB/topics/sensor_combined.h | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e..92812efd4 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + 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[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** -- cgit v1.2.3 From bc64391c538d85e6a1640dac0a76fe0f71a8efc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:59:15 +0100 Subject: AUTOMATED code-style fix on topics. NO MANUAL OR SEMANTIC EDITS --- src/modules/uORB/topics/esc_status.h | 9 +-- src/modules/uORB/topics/filtered_bottom_flow.h | 3 +- src/modules/uORB/topics/home_position.h | 5 +- src/modules/uORB/topics/mission.h | 6 +- src/modules/uORB/topics/navigation_capabilities.h | 2 +- .../uORB/topics/offboard_control_setpoint.h | 5 +- src/modules/uORB/topics/rc_channels.h | 55 +++++++------ src/modules/uORB/topics/subsystem_info.h | 3 +- src/modules/uORB/topics/telemetry_status.h | 22 +++--- .../uORB/topics/vehicle_attitude_setpoint.h | 3 +- .../uORB/topics/vehicle_bodyframe_speed_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_command.h | 91 +++++++++++----------- src/modules/uORB/topics/vehicle_control_debug.h | 9 +-- src/modules/uORB/topics/vehicle_control_mode.h | 5 +- src/modules/uORB/topics/vehicle_global_position.h | 19 +++-- .../topics/vehicle_global_position_set_triplet.h | 3 +- .../uORB/topics/vehicle_global_position_setpoint.h | 3 +- .../uORB/topics/vehicle_global_velocity_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_gps_position.h | 5 +- src/modules/uORB/topics/vehicle_local_position.h | 3 +- .../uORB/topics/vehicle_local_position_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_rates_setpoint.h | 9 +-- src/modules/uORB/topics/vehicle_status.h | 49 ++++++------ src/modules/uORB/topics/vehicle_vicon_position.h | 3 +- 24 files changed, 148 insertions(+), 173 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 11332d7a7..628824efa 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -60,7 +60,7 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { @@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. */ -struct esc_status_s -{ +struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - + uint8_t esc_count; /**< number of connected ESCs */ enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ - + struct { uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h index ab6de2613..c5d545542 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -53,8 +53,7 @@ /** * Filtered bottom flow in bodyframe. */ -struct filtered_bottom_flow_s -{ +struct filtered_bottom_flow_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ float sumx; /**< Integrated bodyframe x flow in meters */ diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0f..09f5140b3 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -53,11 +53,10 @@ /** * GPS home position in WGS84 coordinates. */ -struct home_position_s -{ +struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - + int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a..04cbf73e1 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -67,8 +67,7 @@ enum NAV_CMD { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct mission_item_s -{ +struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees * 1E7 */ double lon; /**< longitude in degrees * 1E7 */ @@ -83,8 +82,7 @@ struct mission_item_s float param4; }; -struct mission_s -{ +struct mission_s { struct mission_item_s *items; unsigned count; }; diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3..3a367b8cf 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -52,7 +52,7 @@ * Airspeed */ struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ }; /** diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 7901b930a..68d3e494b 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -45,12 +45,11 @@ /** * Off-board control inputs. - * + * * Typically sent by a ground control station / joystick or by * some off-board controller via C or SIMULINK. */ -enum OFFBOARD_CONTROL_MODE -{ +enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef15..6eb20efd1 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -52,29 +52,28 @@ */ #define RC_CHANNELS_MAPPED_MAX 15 -/** +/** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of * the channel array chan[]. */ -enum RC_CHANNELS_FUNCTION -{ - THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ +enum RC_CHANNELS_FUNCTION { + THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; /** @@ -85,16 +84,16 @@ enum RC_CHANNELS_FUNCTION struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ + uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ + struct { + float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ + } chan[RC_CHANNELS_MAPPED_MAX]; + uint8_t chan_count; /**< number of valid channels */ - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ + /*String array to store the names of the functions*/ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; + uint8_t rssi; /**< Overall receive signal strength */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index cfe0bf69e..d3a7ce10d 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,8 +50,7 @@ #include #include "../uORB.h" -enum SUBSYSTEM_TYPE -{ +enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, SUBSYSTEM_TYPE_ACC = 2, SUBSYSTEM_TYPE_MAG = 4, diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc..c181ec04f 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -44,10 +44,10 @@ #include "../uORB.h" enum TELEMETRY_STATUS_RADIO_TYPE { - TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, - TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, - TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, - TELEMETRY_STATUS_RADIO_TYPE_WIRE + TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, + TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, + TELEMETRY_STATUS_RADIO_TYPE_WIRE }; /** @@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ - uint8_t noise; /**< background noise level */ - uint8_t remote_noise; /**< remote background noise level */ - uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + unsigned remote_rssi; /**< remote signal strength */ + unsigned rxerrors; /**< receive errors */ + unsigned fixed; /**< count of error corrected packets */ + uint8_t noise; /**< background noise level */ + uint8_t remote_noise; /**< remote background noise level */ + uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a..fda3cd75e 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -52,8 +52,7 @@ /** * vehicle attitude setpoint. */ -struct vehicle_attitude_setpoint_s -{ +struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float roll_body; /**< body angle in NED frame */ diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h index fbfab09f3..fd1ade671 100644 --- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -48,8 +48,7 @@ * @{ */ -struct vehicle_bodyframe_speed_setpoint_s -{ +struct vehicle_bodyframe_speed_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float vx; /**< in m/s */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6..c21a29b13 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -51,43 +51,42 @@ * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, * but can contain additional ones. */ -enum VEHICLE_CMD -{ - VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END=501, /* | */ +enum VEHICLE_CMD { + VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END = 501, /* | */ }; /** @@ -96,14 +95,13 @@ enum VEHICLE_CMD * Should contain all of MAVLink's VEHICLE_CMD_RESULT values * but can contain additional ones. */ -enum VEHICLE_CMD_RESULT -{ - VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ - VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +enum VEHICLE_CMD_RESULT { + VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END = 5, /* | */ }; /** @@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT * @{ */ -struct vehicle_command_s -{ +struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h index 6184284a4..2a45eaccc 100644 --- a/src/modules/uORB/topics/vehicle_control_debug.h +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_control_debug_s -{ +struct vehicle_control_debug_s { uint64_t timestamp; /**< in microseconds since system start */ float roll_p; /**< roll P control part */ @@ -77,9 +76,9 @@ struct vehicle_control_debug_s }; /**< vehicle_control_debug */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_control_debug); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..4c31f92b5 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -38,7 +38,7 @@ /** * @file vehicle_control_mode.h * Definition of the vehicle_control_mode uORB topic. - * + * * All control apps should depend their actions based on the flags set here. */ @@ -59,8 +59,7 @@ * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_control_mode_s -{ +struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e37..4376fa1ba 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -51,16 +51,15 @@ * @{ */ - /** - * Fused global position in WGS84. - * - * This struct contains the system's believ about its position. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s -{ +/** +* Fused global position in WGS84. +* +* This struct contains the system's believ about its position. It is not the raw GPS +* measurement (@see vehicle_gps_position). This topic is usually published by the position +* estimator, which will take more sources of information into account than just GPS, +* e.g. control inputs of the vehicle in a Kalman-filter implementation. +*/ +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 */ bool valid; /**< true if position satisfies validity criteria of estimator */ 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 8516b263f..e8565d887 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -58,8 +58,7 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s -{ +struct vehicle_global_position_set_triplet_s { bool previous_valid; /**< flag indicating previous position is valid */ bool next_valid; /**< flag indicating next position is valid */ diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index a56434d3b..8102e367d 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -58,8 +58,7 @@ * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct vehicle_global_position_setpoint_s -{ +struct vehicle_global_position_setpoint_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ int32_t lat; /**< latitude in degrees * 1E7 */ int32_t lon; /**< longitude in degrees * 1E7 */ diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h index 73961cdfe..5dac877d0 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -47,8 +47,7 @@ * @{ */ -struct vehicle_global_velocity_setpoint_s -{ +struct vehicle_global_velocity_setpoint_s { float vx; /**< in m/s NED */ float vy; /**< in m/s NED */ float vz; /**< in m/s NED */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 1639a08c2..794c3f8bc 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -53,13 +53,12 @@ /** * GPS position in WGS84 coordinates. */ -struct vehicle_gps_position_s -{ +struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ int32_t lat; /**< Latitude in 1E-7 degrees */ int32_t lon; /**< Longitude in 1E-7 degrees */ int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - + uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float p_variance_m; /**< position accuracy estimate m */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 427153782..44c339711 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_local_position_s -{ +struct vehicle_local_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ bool xy_valid; /**< true if x and y are valid */ bool z_valid; /**< true if z is valid */ diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index d24d81e3a..8988a0330 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -49,8 +49,7 @@ * @{ */ -struct vehicle_local_position_setpoint_s -{ +struct vehicle_local_position_setpoint_s { float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 46e62c4b7..9f8b412a7 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_rates_setpoint_s -{ +struct vehicle_rates_setpoint_s { uint64_t timestamp; /**< in microseconds since system start */ float roll; /**< body angular rates in NED frame */ @@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s }; /**< vehicle_rates_setpoint */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7..7ac1a6660 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -131,31 +131,31 @@ enum VEHICLE_MODE_FLAG { * Should match 1:1 MAVLink's MAV_TYPE ENUM */ enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET=9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - VEHICLE_TYPE_KITE=17, /* Kite | */ - VEHICLE_TYPE_ENUM_END=18, /* | */ + VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ + VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ + VEHICLE_TYPE_KITE = 17, /* Kite | */ + VEHICLE_TYPE_ENUM_END = 18, /* | */ }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -168,8 +168,7 @@ enum VEHICLE_BATTERY_WARNING { * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_status_s -{ +struct vehicle_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ @@ -216,7 +215,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h index 0822fa89a..e19a34a5d 100644 --- a/src/modules/uORB/topics/vehicle_vicon_position.h +++ b/src/modules/uORB/topics/vehicle_vicon_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_vicon_position_s -{ +struct vehicle_vicon_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ bool valid; /**< true if position satisfies validity criteria of estimator */ -- cgit v1.2.3 From fd8cae19bdb4ba5e97c810f266f78af1461b51b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:53:30 +0100 Subject: ADC channel mapping fix in sensors app --- src/modules/sensors/sensors.cpp | 11 +++++++---- src/modules/uORB/topics/sensor_combined.h | 3 ++- 2 files changed, 9 insertions(+), 5 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417..6560dec3d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,16 +1169,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of eight channels */ - struct adc_msg_s buf_adc[8]; + /* make space for a maximum of ten channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[10]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; } /* look for specific channels and process the raw voltage to measurement data */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e..918389f38 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,7 +99,8 @@ 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[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[9]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ -- cgit v1.2.3 From d940bdf7f64268d46503cf4b0b996bec3036603e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 17:02:10 +0100 Subject: Consistently making space for a maximum of 12 channels, 10 channels (including reference / temperature) are used / available right now --- src/drivers/stm32/adc/adc.cpp | 2 +- src/modules/sensors/sensors.cpp | 4 ++-- src/modules/uORB/topics/sensor_combined.h | 4 ++-- src/systemcmds/tests/test_adc.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b8..0b8a275e6 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6560dec3d..8da5bb294 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,8 +1169,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of ten channels (to ensure reading all channels at once) */ - struct adc_msg_s buf_adc[10]; + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 918389f38..117105470 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ 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[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[9]; /**< Channel indices of each of these values */ + 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 */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7302f63cc..03391b851 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of ten channels */ - struct adc_msg_s data[10]; + /* make space for a maximum of twelve channels */ + struct adc_msg_s data[12]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); -- cgit v1.2.3 From c266124099fe67dcff5d5f3deeef37acebdc1695 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 23:58:00 +0400 Subject: vehicle_local_position: use double for ref_lat and ref_lon instead of int32, fix related apps --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 ++-- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/uORB/topics/vehicle_local_position.h | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index a4d5560c7..03e6021dc 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -341,8 +341,8 @@ void KalmanNav::updatePublications() _localPos.xy_global = true; _localPos.z_global = true; _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); + _localPos.ref_lat = lat * M_RAD_TO_DEG; + _localPos.ref_lon = lon * M_RAD_TO_DEG; _localPos.ref_alt = 0; _localPos.landed = landed; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 624740237..d297be10a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -670,8 +670,8 @@ handle_message(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_lat = hil_state.lat / 1e7d; + hil_local_pos.ref_lon = hil_state.lon / 1e7d; hil_local_pos.ref_alt = alt0; hil_local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); 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 3d05b37d8..138b9e46e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -576,13 +576,13 @@ MulticopterPositionControl::task_main() was_armed = _control_mode.flag_armed; + update_ref(); + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - update_ref(); - _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; 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 f3b9b9d85..7be5ae979 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -556,8 +556,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2514bafee..24eed228b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1085,8 +1085,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index d567f2e02..aeaf1e244 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -73,8 +73,8 @@ struct vehicle_local_position_s bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ - int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ - int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + double ref_lat; /**< Reference point latitude in degrees */ + double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ /* Distance to surface */ -- cgit v1.2.3 From 068b7526b74c9bbcc31acc28f0d578ed9c0f97b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Mar 2014 00:10:38 +0400 Subject: copyright and code style fixes --- src/lib/geo/geo.c | 76 ++++++++++++++-------- src/lib/geo/geo.h | 24 ++++--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 54 +++++++++------ .../position_estimator_inav_main.c | 22 +++++-- src/modules/uORB/topics/vehicle_local_position.h | 4 +- 5 files changed, 113 insertions(+), 67 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1588a5346..abed7eb1f 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier + * Copyright (C) 2012, 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 @@ -42,6 +39,7 @@ * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -142,7 +140,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -157,7 +155,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -183,7 +181,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa // Additional functions - @author Doug Weibel -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -200,7 +198,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -231,8 +229,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -251,29 +249,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } } in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -329,8 +327,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d } __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, - double lat_next, double lon_next, float alt_next, - float *dist_xy, float *dist_z) + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) { double current_x_rad = lat_next / 180.0 * M_PI; double current_y_rad = lon_next / 180.0 * M_PI; @@ -354,8 +352,8 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, - float x_next, float y_next, float z_next, - float *dist_xy, float *dist_z) + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) { float dx = x_now - x_next; float dy = y_now - y_next; @@ -375,17 +373,23 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing > M_PI_F) { bearing -= M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; + while (bearing <= -M_PI_F) { bearing += M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -399,17 +403,23 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing > M_TWOPI_F) { bearing -= M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; + while (bearing <= 0.0f) { bearing += M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -423,17 +433,23 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing > 180.0f) { bearing -= 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; + while (bearing <= -180.0f) { bearing += 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -447,17 +463,23 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing > 360.0f) { bearing -= 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; + while (bearing <= 0.0f) { bearing += 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index a66bd10e4..0a3f85d97 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier + * Copyright (C) 2012, 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 @@ -42,6 +39,7 @@ * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier + * @author Anton Babushkin * Additional functions - @author Doug Weibel */ @@ -123,30 +121,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); /* * Calculate distance in global frame */ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, - double lat_next, double lon_next, float alt_next, - float *dist_xy, float *dist_z); + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); /* * Calculate distance in local frame (NED) */ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, - float x_next, float y_next, float z_next, - float *dist_xy, float *dist_z); + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); 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 138b9e46e..97357d07a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2013, 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 @@ -40,6 +39,8 @@ * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin */ #include @@ -355,8 +356,9 @@ MulticopterPositionControl::parameters_update(bool force) orb_check(_params_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); @@ -412,33 +414,39 @@ MulticopterPositionControl::poll_subscriptions() orb_check(_att_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } orb_check(_att_sp_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } orb_check(_control_mode_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } orb_check(_manual_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } orb_check(_arming_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } orb_check(_local_pos_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + } } float @@ -550,8 +558,9 @@ MulticopterPositionControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do */ if (pret < 0) { @@ -653,8 +662,9 @@ MulticopterPositionControl::task_main() bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ @@ -663,8 +673,8 @@ MulticopterPositionControl::task_main() /* project setpoint to local frame */ map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); /* update yaw setpoint if needed */ @@ -832,8 +842,9 @@ MulticopterPositionControl::task_main() /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) + if (thr_min < 0.0f) { thr_min = 0.0f; + } } /* limit min lift */ @@ -924,8 +935,9 @@ MulticopterPositionControl::task_main() thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) + if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; + } } /* calculate attitude setpoint from thrust vector */ @@ -1045,18 +1057,21 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_pos_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (pos_control::g_control != nullptr) + if (pos_control::g_control != nullptr) { errx(1, "already running"); + } pos_control::g_control = new MulticopterPositionControl; - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != pos_control::g_control->start()) { delete pos_control::g_control; @@ -1068,8 +1083,9 @@ int mc_pos_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "not running"); + } delete pos_control::g_control; pos_control::g_control = nullptr; 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 7be5ae979..fc394fda6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013, 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 @@ -35,6 +34,8 @@ /** * @file position_estimator_inav_main.c * Model-identification based position estimator for multirotors + * + * @author Anton Babushkin */ #include @@ -95,8 +96,9 @@ static void usage(const char *reason); */ static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); @@ -112,8 +114,9 @@ static void usage(const char *reason) */ int position_estimator_inav_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { if (thread_running) { @@ -125,8 +128,9 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = false; if (argc > 1) - if (!strcmp(argv[2], "-v")) + if (!strcmp(argv[2], "-v")) { verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", @@ -163,8 +167,10 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +{ FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { char *s = malloc(256); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); @@ -173,6 +179,7 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } @@ -505,8 +512,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy - if (!flow_accurate) + if (!flow_accurate) { w_flow *= 0.05f; + } flow_valid = true; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index aeaf1e244..9e9a4519e 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,9 @@ /** * @file vehicle_local_position.h * Definition of the local fused NED position uORB topic. + * + * @author Lorenz Meier + * @author Anton Babushkin */ #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ -- cgit v1.2.3 From 712c72d25bc595d879c2592e0a750bb0a981120f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 12:52:27 +0400 Subject: Optical flow fixes --- src/modules/mavlink/mavlink_receiver.cpp | 1 + .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++++--- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- src/modules/uORB/topics/optical_flow.h | 1 + 4 files changed, 11 insertions(+), 4 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0816814a1..6eec25e4b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -245,6 +245,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) memset(&f, 0, sizeof(f)); f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; 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 caf2f840c..15a88066f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -248,6 +248,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; + hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) hrt_abstime xy_src_time = 0; // time of last available position data @@ -434,6 +435,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* calculate time from previous update */ + float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; + flow_prev = flow.flow_timestamp; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -484,10 +489,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* convert raw flow to angular flow */ + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; + flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f..6892ac496 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 98f0e3fa2..0196ae86b 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -57,6 +57,7 @@ struct optical_flow_s { uint64_t timestamp; /**< in microseconds since system start */ + uint64_t flow_timestamp; /**< timestamp from flow sensor */ int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ -- cgit v1.2.3 From 42bba678939a58e59282d4c63dbdef53f962e0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:07 +0100 Subject: Move changes regarding the filtered airspeed consistently across sensors, use actual air temperature instead of board temperature --- src/drivers/ets_airspeed/ets_airspeed.cpp | 5 ++++- src/modules/sensors/sensors.cpp | 6 +++++- src/modules/uORB/topics/differential_pressure.h | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d27ab9727..d873a1132 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -176,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = (float)diff_pres_pa; + + // XXX we may want to smooth out the readings to remove noise. + report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91a8d5670..13afe27c8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,10 +1031,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + raw.baro_pres_mbar * 1e2f, air_temperature_celcius); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1239,6 +1241,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.temperature = -1000.0f; _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 3592c023c..ff88b04c6 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,7 @@ struct differential_pressure_s { float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From 01f33df70718b7a7dba342fb5920d91b3cd83c09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:04:35 +0100 Subject: Added EKF filter health status reporting, added dynamic in-air reset. --- src/modules/fw_att_pos_estimator/estimator.cpp | 209 +++++++++++++++------ src/modules/fw_att_pos_estimator/estimator.h | 29 ++- .../fw_att_pos_estimator_main.cpp | 65 ++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/estimator_status.h | 81 ++++++++ 5 files changed, 328 insertions(+), 59 deletions(-) create mode 100644 src/modules/uORB/topics/estimator_status.h (limited to 'src/modules/uORB') diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e6f1fee87..bde46ba03 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -74,12 +74,8 @@ bool staticMode = true; ///< boolean true if no position feedback is fused bool useAirspeed = true; ///< boolean true if airspeed data is being used bool useCompass = true; ///< boolean true if magnetometer data is being used -bool velHealth; -bool posHealth; -bool hgtHealth; -bool velTimeout; -bool posTimeout; -bool hgtTimeout; +struct ekf_status_report current_ekf_state; +struct ekf_status_report last_ekf_error; bool numericalProtection = true; @@ -965,9 +961,6 @@ void FuseVelposNED() uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time uint32_t horizRetryTime; - static uint32_t velFailTime = 0; - static uint32_t posFailTime = 0; - static uint32_t hgtFailTime = 0; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1033,16 +1026,16 @@ void FuseVelposNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; } // apply a 5-sigma threshold - velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - velTimeout = (millis() - velFailTime) > horizRetryTime; - if (velHealth || velTimeout) + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) { - velHealth = true; - velFailTime = millis(); + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); } else { - velHealth = false; + current_ekf_state.velHealth = false; } } if (fusePosData) @@ -1053,16 +1046,16 @@ void FuseVelposNED() varInnovVelPos[3] = P[7][7] + R_OBS[3]; varInnovVelPos[4] = P[8][8] + R_OBS[4]; // apply a 10-sigma threshold - posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - posTimeout = (millis() - posFailTime) > horizRetryTime; - if (posHealth || posTimeout) + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) { - posHealth = true; - posFailTime = millis(); + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); } else { - posHealth = false; + current_ekf_state.posHealth = false; } } // test height measurements @@ -1071,37 +1064,37 @@ void FuseVelposNED() hgtInnov = statesAtHgtTime[9] + hgtMea; varInnovVelPos[5] = P[9][9] + R_OBS[5]; // apply a 10-sigma threshold - hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime; - if (hgtHealth || hgtTimeout) + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) { - hgtHealth = true; - hgtFailTime = millis(); + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); } else { - hgtHealth = false; + current_ekf_state.hgtHealth = false; } } // Set range for sequential fusion of velocity and position measurements depending // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && velHealth) + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; fuseData[2] = true; } - if (fuseVelData && fusionModeGPS == 1 && velHealth) + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; } - if (fusePosData && fusionModeGPS <= 2 && posHealth) + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) { fuseData[3] = true; fuseData[4] = true; } - if (fuseHgtData && hgtHealth) + if (fuseHgtData && current_ekf_state.hgtHealth) { fuseData[5] = true; } @@ -1986,7 +1979,7 @@ bool FilterHealthy() // XXX Check state vector for NaNs and ill-conditioning // Check if any of the major inputs timed out - if (posTimeout || velTimeout || hgtTimeout) { + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { return false; } @@ -2042,6 +2035,65 @@ void ResetVelocity(void) } +void FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + /** * Check the filter inputs and bound its operational state * @@ -2051,21 +2103,30 @@ void ResetVelocity(void) * updated, but before any of the fusion steps are * executed. */ -void CheckAndBound() +int CheckAndBound() { // Store the old filter state bool currStaticMode = staticMode; + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED); + + return 1; + } + // Reset the filter if the IMU data is too old if (dtIMU > 0.2f) { + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); // that's all we can do here, return - return; + return 2; } // Check if we're on ground - this also sets static mode. @@ -2077,7 +2138,11 @@ void CheckAndBound() ResetPosition(); ResetHeight(); ResetStoredStates(); + + return 3; } + + return 0; } void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) @@ -2116,28 +2181,13 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float (&initvelNED)[3]) +void InitializeDynamic(float (&initvelNED)[3]) { - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - for (unsigned i = 0; i < data_buffer_size; i++) { - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } + // Clear the init flag + statesInitialised = false; - statetimeStamp[i] = 0; - } + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements float initQuat[4]; @@ -2155,10 +2205,7 @@ void InitialiseFilter(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2187,3 +2234,51 @@ void InitialiseFilter(float (&initvelNED)[3]) summedDelVel.y = 0.0f; summedDelVel.z = 0.0f; } + +void InitialiseFilter(float (&initvelNED)[3]) +{ + //store initial lat,long and height + latRef = gpsLat; + lonRef = gpsLon; + hgtRef = gpsHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED); +} + +void ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 256aff771..55e6eb12e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,6 +134,22 @@ enum GPS_FIX { GPS_FIX_3D = 3 }; +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool statesNaN; + bool covarianceNaN; + bool kalmanGainsNaN; +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -188,11 +204,22 @@ void ConstrainStates(); void ForceSymmetry(); -void CheckAndBound(); +int CheckAndBound(); void ResetPosition(); void ResetVelocity(); +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3]); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e46dffde3..e75b844fd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -144,6 +145,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ + orb_advert_t _estimator_status_pub; /**< status of the estimator */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -260,6 +262,7 @@ FixedwingEstimator::FixedwingEstimator() : _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), + _estimator_status_pub(-1), _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -701,7 +704,67 @@ FixedwingEstimator::task_main() /** * CHECK IF THE INPUT DATA IS SANE */ - CheckAndBound(); + int check = CheckAndBound(); + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 3: + { + const char* str = "switching dynamic / static state"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + } + + // If non-zero, we got a problem + if (check) { + + struct ekf_status_report ekf_report; + + GetLastErrorState(&ekf_report); + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + rep.timestamp = hrt_absolute_time(); + + rep.states_nan = ekf_report.statesNaN; + rep.covariance_nan = ekf_report.covarianceNaN; + rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; + + // Copy all states or at least all that we can fit + int i = 0; + unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + while ((i < ekf_n_states) && (i < max_states)) { + + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } /** diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index fb24de8d1..4b31cc8a4 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -193,3 +193,6 @@ ORB_DEFINE(esc_status, struct esc_status_s); #include "topics/encoders.h" ORB_DEFINE(encoders, struct encoders_s); + +#include "topics/estimator_status.h" +ORB_DEFINE(estimator_status, struct estimator_status_report); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h new file mode 100644 index 000000000..66ca03019 --- /dev/null +++ b/src/modules/uORB/topics/estimator_status.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 estimator_status.h + * Definition of the estimator_status_report uORB topic. + * + * @author Lorenz Meier + */ + +#ifndef ESTIMATOR_STATUS_H_ +#define ESTIMATOR_STATUS_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Estimator status report. + * + * This is a generic status report struct which allows any of the onboard estimators + * to write the internal state to the system log. + * + */ +struct estimator_status_report { + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot */ + float states[32]; /**< Internal filter states */ + float n_states; /**< Number of states effectively used */ + bool states_nan; /**< If set to true, one of the states is NaN */ + bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ + bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(estimator_status); + +#endif -- cgit v1.2.3 From 0874507a44347d10e10606061c411f0b0129dc5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:35:47 +0100 Subject: Added estimator status logging to sdlog2 --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ src/modules/uORB/topics/estimator_status.h | 1 - 3 files changed, 29 insertions(+), 1 deletion(-) (limited to 'src/modules/uORB') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ad709d27d..3f07eea53 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -794,6 +795,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; + struct estimator_status_report estimator_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -825,6 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; + struct log_ESTM_s log_ESTM; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +858,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int battery_sub; int telemetry_sub; int range_finder_sub; + int estimator_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -879,6 +883,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); thread_running = true; @@ -1243,6 +1248,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(DIST); } + /* --- ESTIMATOR STATUS --- */ + if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + log_msg.msg_type = LOG_ESTM_MSG; + unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); + memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); + memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); + log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; + log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; + log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; + log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fe500ad5f..2b41755de 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -299,6 +299,16 @@ struct log_PARM_s { float value; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 132 +struct log_ESTM_s { + float s[32]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -331,6 +341,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), + LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 66ca03019..5530cdb21 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -67,7 +67,6 @@ struct estimator_status_report { bool states_nan; /**< If set to true, one of the states is NaN */ bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ - bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ }; -- cgit v1.2.3 From 83da4ae02dfc61d6a7f80ae40660826fbbca81be Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Mar 2014 00:27:11 +0400 Subject: 'vehicle_global_position' topic updated: removed baro_alt and XXX_valid flags. --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 1 - .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 73 ++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/navigator/navigator_main.cpp | 17 +++-- .../position_estimator_inav_main.c | 48 +++++++------- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sdlog2/sdlog2_messages.h | 6 +- src/modules/uORB/topics/vehicle_global_position.h | 7 +-- 10 files changed, 75 insertions(+), 88 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfcf91e3f..57a03bc84 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.global_valid) { + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 03e6021dc..5cf84542b 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -312,7 +312,6 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; _pos.lat = lat * M_RAD_TO_DEG; _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 10a6cd2c5..c61b6ff3f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5c..7da062961 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */ #define RC_TIMEOUT 100000 #define DIFFPRESS_TIMEOUT 2000000 @@ -919,7 +919,37 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); + + /* check if GPS fix is ok */ + static float hdop_threshold_m = 4.0f; + static float vdop_threshold_m = 8.0f; + + /* update home position */ + if (!status.condition_home_position_valid && updated && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) && + (hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) { + + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; + tune_positive(true); + } /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1067,45 +1097,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* check if GPS fix is ok */ - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { - - /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } } /* start RC input check */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6eec25e4b..2c9cdbf24 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -765,7 +765,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -773,6 +772,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; if (_global_pos_pub > 0) { orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..ef7201790 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -177,7 +177,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -817,13 +817,11 @@ Navigator::task_main() if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -846,8 +844,15 @@ Navigator::task_main() /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } + + _global_pos_valid = true; + + } else { + /* assume that global position is valid if updated in last 20ms */ + _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000; } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -893,9 +898,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); 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 4f7147167..3f1a5d39b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -292,7 +292,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; @@ -340,7 +340,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - global_pos.baro_valid = true; } } } @@ -550,9 +549,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* reproject position estimate to new reference */ float dx, dy; map_projection_project(&ref, home.lat, home.lon, &dx, &dy); - est_x[0] -= dx; - est_y[0] -= dx; - est_z[0] += home.alt - local_pos.ref_alt; + x_est[0] -= dx; + y_est[0] -= dx; + z_est[0] += home.alt - local_pos.ref_alt; } /* update baro offset */ @@ -888,40 +887,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - /* publish global position */ - global_pos.global_valid = local_pos.xy_global; + 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; - if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&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.alt = local_pos.ref_alt - local_pos.z; - /* set valid values even if position is not valid */ - if (local_pos.v_xy_valid) { global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; - } - - if (local_pos.z_global) { - global_pos.alt = local_pos.ref_alt - local_pos.z; - } - - if (local_pos.z_valid) { - global_pos.baro_alt = baro_offset - local_pos.z; - } - - if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; - } - global_pos.yaw = local_pos.yaw; + global_pos.yaw = local_pos.yaw; - global_pos.timestamp = t; + // TODO implement dead-reckoning + global_pos.eph = gps.eph_m; + global_pos.epv = gps.epv_m; - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + if (vehicle_global_position_pub < 0) { + vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + } else { + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c7073eb94..36d309d6c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,8 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + log_msg.body.log_GPOS.eph = buf.global_pos.eph; + log_msg.body.log_GPOS.epv = buf.global_pos.epv; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e27518aa0..fbfca76f7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,8 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; - float baro_alt; - uint8_t flags; + float eph; + float epv; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -317,7 +317,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ff9e98e1c..5c54630e2 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -63,9 +63,6 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool global_valid; /**< true if position satisfies validity criteria of estimator */ - bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ @@ -74,8 +71,8 @@ struct vehicle_global_position_s float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float eph; + float epv; }; /** -- cgit v1.2.3 From 521539897e59f495adaae481911e3a4ac847625e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 26 Mar 2014 14:51:57 -0700 Subject: Simpler state transition code Also fixed ARMING_STATE_ARMED_ERROR->ARMING_STATE_STANDBY_ERROR transition. --- src/modules/commander/state_machine_helper.cpp | 175 +++++++++++-------------- src/modules/uORB/topics/vehicle_status.h | 2 + 2 files changed, 76 insertions(+), 101 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 94ffc31d6..8564747fa 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -68,10 +68,44 @@ static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char* state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, /// current vehicle status + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -82,121 +116,60 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; - } else { - /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; - } else { armed->lockdown = false; } - - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (status->arming_state == ARMING_STATE_STANDBY) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED - || status->hil_state == HIL_STATE_ON) { - - /* sensors need to be initialized for STANDBY state */ - if (status->condition_system_sensors_initialized) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = true; - } - } - - break; - - case ARMING_STATE_ARMED: - - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) { + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // In non-HIL, fail transition if we need safety switch press if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection if (mavlink_fd) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); } - } else { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; + valid_transition = false; } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; } - - break; - - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_ARMED) { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY_ERROR: - - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) - warnx("arming transition rejected"); + if (ret == TRANSITION_DENIED) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]); + } + warnx("arming transition rejected"); + } return ret; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 5d40554fd..0071d3125 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -69,6 +69,8 @@ typedef enum { MAIN_STATE_MAX } main_state_t; +// If you change the order, add or remove arming_state_t states make sure to update the arrays +// in state_machine_helper.cpp as well. typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, -- cgit v1.2.3 From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: 'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately --- src/modules/commander/commander.cpp | 13 +- src/modules/sensors/sensors.cpp | 224 ++++++++++++---------- src/modules/uORB/topics/manual_control_setpoint.h | 2 + src/modules/uORB/topics/rc_channels.h | 1 + 4 files changed, 123 insertions(+), 117 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..edd77231d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && !sp_man.signal_lost && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f..9f816f5e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -474,6 +474,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1283,12 +1284,6 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - if (rc_input.rc_lost) - return; - struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1311,19 +1306,28 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; + manual_control.signal_lost = true; - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* detect RC signal loss */ + /* check flags and require at least four channels to consider the signal valid */ + if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { + /* signal looks good */ + manual_control.signal_lost = false; + + /* check throttle failsafe */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + } } } @@ -1332,10 +1336,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1382,105 +1383,124 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp_last_signal; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = manual_control.signal_lost; - manual_control.timestamp = rc_input.timestamp_last_signal; + if (!manual_control.signal_lost) { + _rc_last_valid = rc_input.timestamp_last_signal; + } - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + _rc.timestamp = _rc_last_valid; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + manual_control.timestamp = _rc_last_valid; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; } - } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } + /* flaps */ + if (_rc.function[FLAPS] >= 0) { - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } + // if (_rc.function[OFFBOARD_MODE] >= 0) { + // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + // } - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + + /* publish actuator_controls_3 topic only if control signal is valid */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1489,21 +1509,13 @@ Sensors::rc_poll() _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ + /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } - - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } } } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..5d1384380 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -51,6 +51,8 @@ struct manual_control_setpoint_s { uint64_t timestamp; + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + float roll; /**< ailerons roll / roll rate input */ float pitch; /**< elevator / pitch / pitch rate */ float yaw; /**< rudder / yaw rate / yaw */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 6eb20efd1..3246a39dd 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -94,6 +94,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** -- cgit v1.2.3 From 1d5f62d890d1d85cef5e0f8e282d8e9e70717d46 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 17:26:07 +0400 Subject: sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic --- src/modules/sensors/sensors.cpp | 165 ++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 37 ++--- 2 files changed, 93 insertions(+), 109 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9f816f5e1..e08d8618f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -135,7 +135,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -169,6 +169,16 @@ private: hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + /** * Gather and publish RC input data. */ @@ -1275,6 +1285,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + } else { + return NAN; + } +} + +switch_pos_t +Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else if (value < STICK_ON_OFF_LIMIT) { + return SWITCH_POS_OFF; + + } else { + return SWITCH_POS_MIDDLE; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1292,13 +1341,6 @@ Sensors::rc_poll() manual_control.pitch = NAN; manual_control.yaw = NAN; manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; - manual_control.flaps = NAN; manual_control.aux1 = NAN; manual_control.aux2 = NAN; @@ -1306,6 +1348,11 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; + manual_control.mode_switch = SWITCH_POS_NONE; + manual_control.return_switch = SWITCH_POS_NONE; + manual_control.assisted_switch = SWITCH_POS_NONE; + manual_control.mission_switch = SWITCH_POS_NONE; + manual_control.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1316,7 +1363,7 @@ Sensors::rc_poll() /* signal looks good */ manual_control.signal_lost = false; - /* check throttle failsafe */ + /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { @@ -1397,89 +1444,23 @@ Sensors::rc_poll() if (!manual_control.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - // if (_rc.function[OFFBOARD_MODE] >= 0) { - // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - // } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } + /* limit controls */ + manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); + manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); + manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); + manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + + /* mode switches */ + manual_control.mode_switch = get_rc_switch_position(MODE); + manual_control.assisted_switch = get_rc_switch_position(ASSISTED); + manual_control.mission_switch = get_rc_switch_position(MISSION); + manual_control.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ actuator_group_3.control[0] = manual_control.roll; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 5d1384380..985d3923f 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -43,6 +43,16 @@ #include #include "../uORB.h" +/** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + /** * @addtogroup topics * @{ @@ -53,32 +63,25 @@ struct manual_control_setpoint_s { bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. */ - - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - - float flaps; /**< flap position */ - + 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 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 */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From 6f38ed3b4b6f266098d0616b6bd3c18ffe082755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:23:34 +0400 Subject: commander, navigator: use updated manual_control_setpoint --- src/modules/commander/commander.cpp | 136 ++++++++++++++----------------- src/modules/navigator/navigator_main.cpp | 100 ++++++++--------------- src/modules/uORB/topics/vehicle_status.h | 28 ------- 3 files changed, 95 insertions(+), 169 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edd77231d..7e469c9c1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -112,8 +113,7 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) @@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update subsystem */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -1135,7 +1147,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 < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1153,7 +1165,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 < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 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."); @@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { @@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + case SWITCH_POS_MIDDLE: // ASSISTED + if (sp_man->assisted_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT print_reject_mode(status, "EASY"); @@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status) res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (sp_man->assisted_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..b7f26198b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -690,84 +690,46 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; - - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; + + case NAV_STATE_MISSION: + request_mission_if_available(); + break; + + case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } - stick_mode = true; + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; - } + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); } } @@ -775,6 +737,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56be4d241..48af4c9e2 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -93,29 +93,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - MODE_SWITCH_AUTO -} mode_switch_pos_t; - -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_EASY -} assisted_switch_pos_t; - -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,11 +164,6 @@ struct vehicle_status_s { bool is_rotary_wing; - mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; - assisted_switch_pos_t assisted_switch; - mission_switch_pos_t mission_switch; - bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; -- cgit v1.2.3 From ea5279389f2b13110735083f511afb630ef5e3d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:16:51 +1100 Subject: uORB: added an ORB topic for system_power holds power supply state and 5V rail voltage on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/system_power.h | 71 ++++++++++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/system_power.h (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a4..c8a589bb7 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 000000000..7763b6004 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 system_power.h + * + * Definition of the system_power voltage and power status uORB topic. + */ + +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * voltage and power supply status + */ +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif -- cgit v1.2.3 From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 +++++++++++++++++++------ src/modules/uORB/topics/differential_pressure.h | 9 +++-- 2 files changed, 43 insertions(+), 15 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383..58b128948 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6..01e14cda9 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From b00b70aeb2b7f20e74ca185e357e796f54031640 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:06:10 +0400 Subject: manual_control_setpoint: signal_lost flag removed, sensors: bugs fixed --- src/modules/sensors/sensors.cpp | 36 +++++++---------------- src/modules/uORB/topics/manual_control_setpoint.h | 2 -- 2 files changed, 10 insertions(+), 28 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 37255c4bf..5b826a919 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -214,7 +214,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ - struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -468,21 +467,6 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); - memset(&_manual, 0, sizeof(_manual)); - - /* set NANs instead of zeroes */ - _manual.roll = NAN; - _manual.pitch = NAN; - _manual.yaw = NAN; - _manual.throttle = NAN; - _manual.flaps = NAN; - _manual.aux1 = NAN; - _manual.aux2 = NAN; - _manual.aux3 = NAN; - _manual.aux4 = NAN; - _manual.aux5 = NAN; - - _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1427,10 +1411,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } /* copy from mapped manual control to control group 3 */ @@ -1439,14 +1423,14 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_publication; - 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[4] = _manual.flaps; - actuator_group_3.control[5] = _manual.aux1; - actuator_group_3.control[6] = _manual.aux2; - actuator_group_3.control[7] = _manual.aux3; + 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[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ if (_actuator_group_3_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 985d3923f..2b3a337b2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -61,8 +61,6 @@ typedef enum { struct manual_control_setpoint_s { uint64_t timestamp; - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. -- cgit v1.2.3 From 6319ec2036c52f39a6fded6480836fe79e3ba35f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:34 +0200 Subject: Add celsius air temperature field to airspeed --- src/modules/uORB/topics/airspeed.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758f..d2ee754cd 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** -- cgit v1.2.3 From a4b10bab3042d653c5f59e704cb58008f7551401 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: navigator: wrong mission topic was copied, clearer naming of offboard mission now --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f4a4ce86c..2416a1264 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -832,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a94b6671..bcb3c8d0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9594c4c6a..ef4bc1def 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -105,7 +105,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif -- cgit v1.2.3 From 7e621070ca0f002e2e1ccd863c31a24166ece0c2 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 18:23:27 -0700 Subject: renamed mission_switch to loiter_switch and assisted_switch to easy_switch --- src/modules/commander/commander.cpp | 10 +++++----- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 24 +++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 4 ++-- src/modules/uORB/topics/rc_channels.h | 4 ++-- 5 files changed, 23 insertions(+), 23 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..ee818d0f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1296,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { + if (sp_man->easy_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) { @@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { + if (sp_man->easy_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c04e176a1..a34f81923 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); /** * Mission switch channel mapping. @@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 04b74a6f5..469fc5ca0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,8 +253,8 @@ private: int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_easy_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,8 +296,8 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; - param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_easy_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -507,8 +507,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,11 +650,11 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -681,8 +681,8 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1415,8 +1415,8 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.easy_switch = get_rc_switch_position(EASY); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b2..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,8 +78,8 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, - MISSION = 7, + EASY = 6, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, -- cgit v1.2.3 From 63c50676f9757f18dfca9ec3735ce59a045cea33 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Apr 2014 22:38:19 +0200 Subject: MISSION switch renamed to LOITER --- src/modules/commander/commander.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 12 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- 5 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ac433b1c7..67b9d16e8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a1f2a4ad5..bc49f5c85 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); /** - * Mission switch channel mapping. + * Loiter switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ba233dfd0..41e2148ac 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -254,7 +254,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -297,7 +297,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -508,7 +508,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -654,7 +654,7 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -682,7 +682,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1416,7 +1416,7 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b2..a23d89cd2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -79,7 +79,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd..c168b2fac 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION { MODE = 4, RETURN = 5, ASSISTED = 6, - MISSION = 7, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, -- cgit v1.2.3 From 6a7b104c2baad7527d35736979ddd1352bf4119d Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 13:12:28 -0700 Subject: compiles --- src/modules/commander/commander.cpp | 5 +--- src/modules/sensors/sensors.cpp | 34 +---------------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 --- src/modules/uORB/topics/rc_channels.h | 4 --- 4 files changed, 2 insertions(+), 45 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5f05c991..50380107d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1228,11 +1228,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { -<<<<<<< HEAD + /* LOITER switch */ -======= - /* MISSION switch */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a750db12..6449c5283 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -255,11 +255,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; -<<<<<<< HEAD int rc_map_easy_sw; -======= - int rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -313,11 +309,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; -<<<<<<< HEAD param_t rc_map_easy_sw; -======= - param_t rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -534,11 +526,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ -<<<<<<< HEAD _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); -======= - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -679,7 +667,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { @@ -690,21 +678,12 @@ Sensors::parameters_update() warnx("%s", paramerr); } -<<<<<<< HEAD if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { - warnx(paramerr); - } - - if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); -======= - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx("%s", paramerr); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -745,11 +724,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; -<<<<<<< HEAD _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; -======= - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1502,17 +1477,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ -<<<<<<< HEAD manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); -======= - manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c61987df6..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,11 +78,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ -<<<<<<< HEAD switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ -======= - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index df651e78d..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,11 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, -<<<<<<< HEAD EASY = 6, -======= - ASSISTED = 6, ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, -- cgit v1.2.3 From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297f..aa9c64502 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d..a99456370 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d..b440e561b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d3460..962d2804c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c7..21e36a87d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..fafab9bfe 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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 7f13df785..d5a68e21f 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 @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_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, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_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, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..108ef8ad4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59..015d8ad16 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..2cb4fc900 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bdd..59bd99db7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: Replaces poshold/althold with posctrl/altctrl --- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) (limited to 'src/modules/uORB') diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc3..bd3fc66e7 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c64502..974e20ca2 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370..be3e6d269 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b..18586053b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804c..e6274fb89 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87d..3b6d95135 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfe..dc82ee475 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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 d5a68e21f..024dd98ec 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 @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _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, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _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, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad4..38a5433ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad16..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900..6d46db9bd 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db7..02890b452 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Sun, 11 May 2014 12:54:15 +0200 Subject: Use "POSCTL" switch name consistently --- src/modules/sensors/sensor_params.c | 6 +++--- src/modules/sensors/sensors.cpp | 26 +++++++++++++------------- src/modules/uORB/topics/rc_channels.h | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 873fff872..0dfc3d9e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -599,7 +599,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); /** - * Assist switch channel mapping. + * Posctl switch channel mapping. * * @min 0 * @max 18 @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting posctrl mode + * Threshold for selecting posctl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Sun, 11 May 2014 13:35:05 +0200 Subject: ALTCTRL/POSCTRL renamed to ALTCTL/POSCTL --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 12 +++--- src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 47 insertions(+), 47 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 974e20ca2..c41d8f242 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5124a31f..8c7f6270d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { - /* ALTCTRL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTRL"; - main_states_str[2] = "POSCTRL"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assist switch is on posctrl position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctrl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTRL); + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTCTRL - print_reject_mode(status, "POSCTRL"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctrl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTRL"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); } // else fallback to MANUAL @@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTRL (POSCTRL likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1674,7 +1674,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1685,7 +1685,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 18586053b..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -320,15 +320,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); @@ -336,15 +336,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e6274fb89..a83c81850 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTCTRL, - PX4_CUSTOM_MAIN_MODE_POSCTRL, + PX4_CUSTOM_MAIN_MODE_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3b6d95135..97a214c33 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 596e286a4..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9a7e636c3..b8879157e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTCTRL) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_POSCTRL) { + } else if (status->main_state == MAIN_STATE_POSCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 6d46db9bd..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 727be9492..ac394786d 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -77,8 +77,8 @@ struct manual_control_setpoint_s { float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */ - switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */ + switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */ + switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */ switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f401140ae..f56355246 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,8 +63,8 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTRL, - MAIN_STATE_POSCTRL, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_MAX } main_state_t; -- cgit v1.2.3 From c131e4cada4ded558f8eba52261fb87863f3ee48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 11 May 2014 23:46:12 +0200 Subject: manual_control_setpoint: comments fixed --- src/modules/uORB/topics/manual_control_setpoint.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index ac394786d..ee159b998 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -76,10 +76,10 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */ - switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */ - switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ + 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 */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From d9333a199354c0dfbe742cf396a90dc064cc5195 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:20:40 +0200 Subject: manual control setpoint: rename variables --- src/modules/uORB/topics/manual_control_setpoint.h | 36 +++++++++++++++-------- 1 file changed, 23 insertions(+), 13 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index a23d89cd2..593e9453f 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -64,22 +64,32 @@ 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 */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to 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; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From 15699549a21e08e9bf384dba7a1b65d092f1cb9a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 13:35:11 +0200 Subject: manual control setpoint: add comment about sign --- src/modules/uORB/topics/manual_control_setpoint.h | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) (limited to 'src/modules/uORB') diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index a8fb795f4..19a29635b 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -71,13 +71,20 @@ struct manual_control_setpoint_s { * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle */ + 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 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 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 rotation around the vertical + 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 */ -- cgit v1.2.3