Commander helper functions implementations. More...

#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include "commander_helper.h"
#include "DevMgr.hpp"

Include dependency graph for commander_helper.cpp:

Macros
#define	VEHICLE_TYPE_FIXED_WING 1

#define	VEHICLE_TYPE_QUADROTOR 2

#define	VEHICLE_TYPE_COAXIAL 3

#define	VEHICLE_TYPE_HELICOPTER 4

#define	VEHICLE_TYPE_GROUND_ROVER 10

#define	VEHICLE_TYPE_HEXAROTOR 13

#define	VEHICLE_TYPE_OCTOROTOR 14

#define	VEHICLE_TYPE_TRICOPTER 15

#define	VEHICLE_TYPE_VTOL_DUOROTOR 19

#define	VEHICLE_TYPE_VTOL_QUADROTOR 20

#define	VEHICLE_TYPE_VTOL_TILTROTOR 21

#define	VEHICLE_TYPE_VTOL_RESERVED2 22

#define	VEHICLE_TYPE_VTOL_RESERVED3 23

#define	VEHICLE_TYPE_VTOL_RESERVED4 24

#define	VEHICLE_TYPE_VTOL_RESERVED5 25

#define	BLINK_MSG_TIME 700000

Functions
bool	is_multirotor (const struct vehicle_status_s *current_status)

bool	is_rotary_wing (const struct vehicle_status_s *current_status)

bool	is_vtol (const struct vehicle_status_s *current_status)

bool	is_vtol_tailsitter (const struct vehicle_status_s *current_status)

bool	is_fixed_wing (const struct vehicle_status_s *current_status)

bool	is_ground_rover (const struct vehicle_status_s *current_status)

int	buzzer_init ()

void	buzzer_deinit ()

void	set_tune_override (int tune)

void	set_tune (int tune)

void	tune_home_set (bool use_buzzer)

void	tune_mission_ok (bool use_buzzer)

void	tune_mission_fail (bool use_buzzer)

void	tune_positive (bool use_buzzer)
	Blink green LED and play positive tune (if use_buzzer == true). More...

void	tune_neutral (bool use_buzzer)
	Blink white LED and play neutral tune (if use_buzzer == true). More...

void	tune_negative (bool use_buzzer)
	Blink red LED and play negative tune (if use_buzzer == true). More...

void	tune_failsafe (bool use_buzzer)

int	blink_msg_state ()

int	led_init ()

void	led_deinit ()

int	led_toggle (int led)

int	led_on (int led)

int	led_off (int led)

void	rgbled_set_color_and_mode (uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)

void	rgbled_set_color_and_mode (uint8_t color, uint8_t mode)
	set the LED color & mode More...

Variables
static hrt_abstime	blink_msg_end = 0

static hrt_abstime	tune_end = 0

static int	tune_current = TONE_STOP_TUNE

static unsigned int	tune_durations [TONE_NUMBER_OF_TUNES] {}

static DevHandle	h_leds

static DevHandle	h_buzzer

static led_control_s	led_control {}

static orb_advert_t	led_control_pub = nullptr

static tune_control_s	tune_control {}

static orb_advert_t	tune_control_pub = nullptr

Detailed Description

Commander helper functions implementations.

Author: Thomas Gubler thoma.nosp@m.sgub.nosp@m.ler@s.nosp@m.tude.nosp@m.nt.et.nosp@m.hz.c.nosp@m.h; Julian Oes julia.nosp@m.n@oe.nosp@m.s.ch; Anton Babushkin anton.nosp@m..bab.nosp@m.ushki.nosp@m.n@me.nosp@m..com

Definition in file commander_helper.cpp.

Macro Definition Documentation

◆ BLINK_MSG_TIME

#define BLINK_MSG_TIME 700000

Definition at line 86 of file commander_helper.cpp.

Referenced by tune_failsafe(), tune_home_set(), tune_mission_fail(), tune_mission_ok(), tune_negative(), tune_neutral(), and tune_positive().

◆ VEHICLE_TYPE_COAXIAL

#define VEHICLE_TYPE_COAXIAL 3

Definition at line 72 of file commander_helper.cpp.

Referenced by is_rotary_wing().

◆ VEHICLE_TYPE_FIXED_WING

#define VEHICLE_TYPE_FIXED_WING 1

Definition at line 70 of file commander_helper.cpp.

Referenced by land_detector::VtolLandDetector::_get_landed_state(), land_detector::VtolLandDetector::_get_maybe_landed_state(), RTL::advance_rtl(), MavlinkReceiver::fill_thrust(), Navigator::force_vtol(), Navigator::get_altitude_acceptance_radius(), Navigator::get_default_altitude_acceptance_radius(), MavlinkReceiver::handle_message_set_attitude_target(), Mission::index_closest_mission_item(), is_fixed_wing(), MissionBlock::is_mission_item_reached(), FailureDetector::isAttitudeStabilized(), StateMachineHelperTest::mainStateTransitionTest(), PreFlightCheck::preflightCheck(), Commander::run(), Ekf2::Run(), FixedwingPositionControl::Run(), MavlinkStreamAttitudeQuaternion::send(), Commander::set_main_state_rc(), Mission::set_mission_items(), set_nav_state(), stabilization_required(), MulticopterPositionControl::start_flight_task(), Commander::update_control_mode(), MavlinkStreamHighLatency2::update_vehicle_status(), and FixedwingAttitudeControl::vehicle_manual_poll().

◆ VEHICLE_TYPE_GROUND_ROVER

#define VEHICLE_TYPE_GROUND_ROVER 10

Definition at line 74 of file commander_helper.cpp.

Referenced by is_ground_rover().

◆ VEHICLE_TYPE_HELICOPTER

#define VEHICLE_TYPE_HELICOPTER 4

Definition at line 73 of file commander_helper.cpp.

Referenced by is_rotary_wing().

◆ VEHICLE_TYPE_HEXAROTOR

#define VEHICLE_TYPE_HEXAROTOR 13

Definition at line 75 of file commander_helper.cpp.

Referenced by is_multirotor().

◆ VEHICLE_TYPE_OCTOROTOR

#define VEHICLE_TYPE_OCTOROTOR 14

Definition at line 76 of file commander_helper.cpp.

Referenced by is_multirotor().

◆ VEHICLE_TYPE_QUADROTOR

#define VEHICLE_TYPE_QUADROTOR 2

Definition at line 71 of file commander_helper.cpp.

Referenced by is_multirotor().

◆ VEHICLE_TYPE_TRICOPTER

#define VEHICLE_TYPE_TRICOPTER 15

Definition at line 77 of file commander_helper.cpp.

Referenced by is_multirotor().

◆ VEHICLE_TYPE_VTOL_DUOROTOR

#define VEHICLE_TYPE_VTOL_DUOROTOR 19

Definition at line 78 of file commander_helper.cpp.

Referenced by is_vtol(), and is_vtol_tailsitter().

◆ VEHICLE_TYPE_VTOL_QUADROTOR

#define VEHICLE_TYPE_VTOL_QUADROTOR 20

Definition at line 79 of file commander_helper.cpp.

Referenced by is_vtol(), and is_vtol_tailsitter().

◆ VEHICLE_TYPE_VTOL_RESERVED2

#define VEHICLE_TYPE_VTOL_RESERVED2 22

Definition at line 81 of file commander_helper.cpp.

Referenced by is_vtol().

◆ VEHICLE_TYPE_VTOL_RESERVED3

#define VEHICLE_TYPE_VTOL_RESERVED3 23

Definition at line 82 of file commander_helper.cpp.

Referenced by is_vtol().

◆ VEHICLE_TYPE_VTOL_RESERVED4

#define VEHICLE_TYPE_VTOL_RESERVED4 24

Definition at line 83 of file commander_helper.cpp.

Referenced by is_vtol().

◆ VEHICLE_TYPE_VTOL_RESERVED5

#define VEHICLE_TYPE_VTOL_RESERVED5 25

Definition at line 84 of file commander_helper.cpp.

Referenced by is_vtol().

◆ VEHICLE_TYPE_VTOL_TILTROTOR

#define VEHICLE_TYPE_VTOL_TILTROTOR 21

Definition at line 80 of file commander_helper.cpp.

Referenced by is_vtol().

Function Documentation

◆ blink_msg_state()

int blink_msg_state ( )

Definition at line 274 of file commander_helper.cpp.

References blink_msg_end, and hrt_absolute_time().

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ buzzer_deinit()

void buzzer_deinit ( void )

Definition at line 155 of file commander_helper.cpp.

References orb_unadvertise(), and tune_control_pub.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ buzzer_init()

int buzzer_init ( void )

Definition at line 141 of file commander_helper.cpp.

References orb_advertise_queue(), ORB_ID, TONE_ARMING_WARNING_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_BATTERY_WARNING_SLOW_TUNE, TONE_HOME_SET, TONE_NOTIFY_NEGATIVE_TUNE, TONE_NOTIFY_NEUTRAL_TUNE, TONE_NOTIFY_POSITIVE_TUNE, TONE_SINGLE_BEEP_TUNE, tune_control, tune_control_pub, and tune_durations.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_fixed_wing()

bool is_fixed_wing ( const struct vehicle_status_s * current_status )

Definition at line 119 of file commander_helper.cpp.

References vehicle_status_s::system_type, and VEHICLE_TYPE_FIXED_WING.

Referenced by Commander::run(), Ekf2::Run(), EstimatorInterface::set_is_fixed_wing(), and FixedwingAttitudeControl::vehicle_manual_poll().

Here is the caller graph for this function:

◆ is_ground_rover()

bool is_ground_rover ( const struct vehicle_status_s * current_status )

Definition at line 124 of file commander_helper.cpp.

References vehicle_status_s::system_type, and VEHICLE_TYPE_GROUND_ROVER.

Referenced by Commander::handle_command(), and Commander::run().

Here is the caller graph for this function:

◆ is_multirotor()

bool is_multirotor ( const struct vehicle_status_s * current_status )

Definition at line 88 of file commander_helper.cpp.

References vehicle_status_s::system_type, VEHICLE_TYPE_HEXAROTOR, VEHICLE_TYPE_OCTOROTOR, VEHICLE_TYPE_QUADROTOR, and VEHICLE_TYPE_TRICOPTER.

Referenced by is_rotary_wing().

Here is the caller graph for this function:

◆ is_rotary_wing()

bool is_rotary_wing ( const struct vehicle_status_s * current_status )

Definition at line 96 of file commander_helper.cpp.

References is_multirotor(), vehicle_status_s::system_type, VEHICLE_TYPE_COAXIAL, and VEHICLE_TYPE_HELICOPTER.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_vtol()

bool is_vtol ( const struct vehicle_status_s * current_status )

Definition at line 102 of file commander_helper.cpp.

References vehicle_status_s::system_type, VEHICLE_TYPE_VTOL_DUOROTOR, VEHICLE_TYPE_VTOL_QUADROTOR, VEHICLE_TYPE_VTOL_RESERVED2, VEHICLE_TYPE_VTOL_RESERVED3, VEHICLE_TYPE_VTOL_RESERVED4, VEHICLE_TYPE_VTOL_RESERVED5, and VEHICLE_TYPE_VTOL_TILTROTOR.

Referenced by Commander::run().

Here is the caller graph for this function:

◆ is_vtol_tailsitter()

bool is_vtol_tailsitter ( const struct vehicle_status_s * current_status )

Definition at line 113 of file commander_helper.cpp.

References vehicle_status_s::system_type, VEHICLE_TYPE_VTOL_DUOROTOR, and VEHICLE_TYPE_VTOL_QUADROTOR.

Referenced by Commander::run().

Here is the caller graph for this function:

◆ led_deinit()

void led_deinit ( void )

Definition at line 327 of file commander_helper.cpp.

References h_leds, led_control_pub, and orb_unadvertise().

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ led_init()

int led_init ( void )

Definition at line 288 of file commander_helper.cpp.

References blink_msg_end, h_leds, hrt_absolute_time(), LED0_DEVICE_PATH, LED_AMBER, LED_BLUE, led_control, led_control_pub, LED_GREEN, led_control_s::led_mask, led_off(), LED_ON, led_control_s::mode, orb_advertise_queue(), ORB_ID, led_control_s::priority, and led_control_s::timestamp.

Referenced by LED::_measure(), LED::init(), and Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ led_off()

int led_off ( int led )

Definition at line 343 of file commander_helper.cpp.

References h_leds, and LED_OFF.

Referenced by LED::devIOCTL(), LED::ioctl(), and led_init().

Here is the caller graph for this function:

◆ led_on()

int led_on ( int led )

Definition at line 338 of file commander_helper.cpp.

References h_leds, and LED_ON.

Referenced by LED::devIOCTL(), and LED::ioctl().

Here is the caller graph for this function:

◆ led_toggle()

int led_toggle ( int led )

Definition at line 333 of file commander_helper.cpp.

References h_leds, and LED_TOGGLE.

Referenced by LED::devIOCTL(), and LED::ioctl().

Here is the caller graph for this function:

◆ rgbled_set_color_and_mode() [1/2]

void rgbled_set_color_and_mode	(	uint8_t	color,
		uint8_t	mode,
		uint8_t	blinks,
		uint8_t	prio
	)

Definition at line 348 of file commander_helper.cpp.

References led_control_s::color, hrt_absolute_time(), led_control, led_control_pub, led_control_s::mode, led_control_s::num_blinks, ORB_ID, orb_publish(), led_control_s::priority, and led_control_s::timestamp.

Referenced by calibrate_from_orientation(), control_status_leds(), rgbled_set_color_and_mode(), Commander::run(), tune_failsafe(), tune_home_set(), tune_mission_fail(), tune_mission_ok(), tune_negative(), tune_neutral(), and tune_positive().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rgbled_set_color_and_mode() [2/2]

void rgbled_set_color_and_mode	(	uint8_t	color,
		uint8_t	mode
	)

set the LED color & mode

Parameters

color

See also: led_control_s::COLOR_*

Parameters

mode

See also: led_control_s::MODE_*

Definition at line 358 of file commander_helper.cpp.

References rgbled_set_color_and_mode().

Here is the call graph for this function:

◆ set_tune()

void set_tune ( int tune )

Definition at line 169 of file commander_helper.cpp.

References hrt_absolute_time(), ORB_ID, orb_publish(), tune_control_s::timestamp, tune_control, tune_control_pub, tune_current, tune_durations, tune_end, tune_control_s::tune_id, tune_control_s::tune_override, and tune_control_s::volume.

Referenced by calibrate_from_orientation(), mag_calibration_worker(), Commander::run(), tune_failsafe(), tune_home_set(), tune_mission_fail(), tune_mission_ok(), tune_negative(), tune_neutral(), and tune_positive().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_tune_override()

void set_tune_override ( int tune )

Definition at line 160 of file commander_helper.cpp.

References hrt_absolute_time(), ORB_ID, orb_publish(), tune_control_s::timestamp, tune_control, tune_control_pub, tune_control_s::tune_id, tune_control_s::tune_override, and tune_control_s::volume.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_failsafe()

void tune_failsafe ( bool use_buzzer )

Definition at line 264 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_BATTERY_WARNING_FAST_TUNE.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_home_set()

void tune_home_set ( bool use_buzzer )

Definition at line 195 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_HOME_SET.

Referenced by Commander::set_home_position().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_mission_fail()

void tune_mission_fail ( bool use_buzzer )

Definition at line 215 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_NOTIFY_NEGATIVE_TUNE.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_mission_ok()

void tune_mission_ok ( bool use_buzzer )

Definition at line 205 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_NOTIFY_NEUTRAL_TUNE.

Referenced by Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_negative()

void tune_negative ( bool use_buzzer )

Blink red LED and play negative tune (if use_buzzer == true).

Definition at line 254 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_NOTIFY_NEGATIVE_TUNE.

Referenced by answer_command(), arm_disarm(), calibrate_answer_command(), commander_low_prio_loop(), print_reject_arm(), print_reject_mode(), and Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_neutral()

void tune_neutral ( bool use_buzzer )

Blink white LED and play neutral tune (if use_buzzer == true).

Definition at line 241 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_NOTIFY_NEUTRAL_TUNE.

Referenced by do_airspeed_calibration(), and Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ tune_positive()

void tune_positive ( bool use_buzzer )

Blink green LED and play positive tune (if use_buzzer == true).

Definition at line 228 of file commander_helper.cpp.

References blink_msg_end, BLINK_MSG_TIME, hrt_absolute_time(), rgbled_set_color_and_mode(), set_tune(), and TONE_NOTIFY_POSITIVE_TUNE.

Referenced by calibrate_answer_command(), commander_low_prio_loop(), and Commander::run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ blink_msg_end

hrt_abstime blink_msg_end = 0

static

Definition at line 129 of file commander_helper.cpp.

Referenced by blink_msg_state(), led_init(), tune_failsafe(), tune_home_set(), tune_mission_fail(), tune_mission_ok(), tune_negative(), tune_neutral(), and tune_positive().

◆ h_buzzer

DevHandle h_buzzer

static

Definition at line 135 of file commander_helper.cpp.

◆ h_leds

DevHandle h_leds

static

Definition at line 134 of file commander_helper.cpp.

Referenced by led_deinit(), led_init(), led_off(), led_on(), and led_toggle().

◆ led_control

led_control_s led_control {}

static

Definition at line 136 of file commander_helper.cpp.

Referenced by TemperatureCalibration::exit_task(), led_control_main(), led_init(), rgbled_set_color_and_mode(), run_led_test1(), LedController::NextState::set(), LedController::PerLedData::set(), TemperatureCalibration::task_main(), and LedController::update().

◆ led_control_pub

orb_advert_t led_control_pub = nullptr

static

Definition at line 137 of file commander_helper.cpp.

Referenced by led_deinit(), led_init(), publish_led_control(), and rgbled_set_color_and_mode().

◆ tune_control

tune_control_s tune_control {}

static

Definition at line 138 of file commander_helper.cpp.

Referenced by buzzer_init(), MK::play_beep(), set_tune(), set_tune_override(), MK::task_main(), test_tone(), and tune_control_main().

◆ tune_control_pub

orb_advert_t tune_control_pub = nullptr

static

Definition at line 139 of file commander_helper.cpp.

Referenced by buzzer_deinit(), buzzer_init(), set_tune(), set_tune_override(), and test_tone().

◆ tune_current

int tune_current = TONE_STOP_TUNE

static

Definition at line 131 of file commander_helper.cpp.

Referenced by set_tune().

◆ tune_durations

unsigned int tune_durations[TONE_NUMBER_OF_TUNES] {}

static

Definition at line 132 of file commander_helper.cpp.

Referenced by buzzer_init(), and set_tune().

◆ tune_end

hrt_abstime tune_end = 0

static

Definition at line 130 of file commander_helper.cpp.

Referenced by set_tune().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ BLINK_MSG_TIME

◆ VEHICLE_TYPE_COAXIAL

◆ VEHICLE_TYPE_FIXED_WING

◆ VEHICLE_TYPE_GROUND_ROVER

◆ VEHICLE_TYPE_HELICOPTER

◆ VEHICLE_TYPE_HEXAROTOR

◆ VEHICLE_TYPE_OCTOROTOR

◆ VEHICLE_TYPE_QUADROTOR

◆ VEHICLE_TYPE_TRICOPTER

◆ VEHICLE_TYPE_VTOL_DUOROTOR

◆ VEHICLE_TYPE_VTOL_QUADROTOR

◆ VEHICLE_TYPE_VTOL_RESERVED2

◆ VEHICLE_TYPE_VTOL_RESERVED3

◆ VEHICLE_TYPE_VTOL_RESERVED4

◆ VEHICLE_TYPE_VTOL_RESERVED5

◆ VEHICLE_TYPE_VTOL_TILTROTOR

Function Documentation

◆ blink_msg_state()

◆ buzzer_deinit()

◆ buzzer_init()

◆ is_fixed_wing()

◆ is_ground_rover()

◆ is_multirotor()

◆ is_rotary_wing()

◆ is_vtol()

◆ is_vtol_tailsitter()

◆ led_deinit()

◆ led_init()

◆ led_off()

◆ led_on()

◆ led_toggle()

◆ rgbled_set_color_and_mode() [1/2]

◆ rgbled_set_color_and_mode() [2/2]

◆ set_tune()

◆ set_tune_override()

◆ tune_failsafe()

◆ tune_home_set()

◆ tune_mission_fail()

◆ tune_mission_ok()

◆ tune_negative()

◆ tune_neutral()

◆ tune_positive()

Variable Documentation

◆ blink_msg_end

◆ h_buzzer

◆ h_leds

◆ led_control

◆ led_control_pub

◆ tune_control

◆ tune_control_pub

◆ tune_current

◆ tune_durations

◆ tune_end