PX4 Firmware
PX4 Autopilot Software http://px4.io
messages.cpp File Reference
#include "messages.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <lib/ecl/geo/geo.h>
#include <unistd.h>
#include <px4_platform_common/defines.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
Include dependency graph for messages.cpp:

Go to the source code of this file.

Macros

#define BOARD_TEMP_OFFSET_DEG   5
 

Functions

void init_sub_messages (void)
 
void init_pub_messages (void)
 
void build_gam_request (uint8_t *buffer, size_t *size)
 
void publish_gam_message (const uint8_t *buffer)
 
void build_eam_response (uint8_t *buffer, size_t *size)
 
void build_gam_response (uint8_t *buffer, size_t *size)
 
void build_gps_response (uint8_t *buffer, size_t *size)
 
void convert_to_degrees_minutes_seconds (double val, int *deg, int *min, int *sec)
 

Variables

static int _battery_sub = -1
 
static int _gps_sub = -1
 
static int _home_sub = -1
 
static int _airdata_sub = -1
 
static int _airspeed_sub = -1
 
static int _esc_sub = -1
 
static orb_advert_t _esc_pub = nullptr
 
static bool _home_position_set = false
 
static double _home_lat = 0.0d
 
static double _home_lon = 0.0d
 

Macro Definition Documentation

◆ BOARD_TEMP_OFFSET_DEG

#define BOARD_TEMP_OFFSET_DEG   5

Definition at line 58 of file messages.cpp.

Referenced by build_eam_response().

Function Documentation

◆ build_eam_response()

void build_eam_response ( uint8_t *  buffer,
size_t *  size 
)

Definition at line 132 of file messages.cpp.

References _airdata_sub, _airspeed_sub, _battery_sub, eam_module_msg::altitude_H, eam_module_msg::altitude_L, vehicle_air_data_s::baro_alt_meter, vehicle_air_data_s::baro_temp_celcius, BOARD_TEMP_OFFSET_DEG, EAM_SENSOR_ID, eam_module_msg::eam_sensor_id, EAM_SENSOR_TEXT_ID, airspeed_s::indicated_airspeed_m_s, eam_module_msg::main_voltage_L, msg, orb_copy(), ORB_ID, eam_module_msg::sensor_text_id, eam_module_msg::speed_H, eam_module_msg::speed_L, eam_module_msg::start, START_BYTE, eam_module_msg::stop, STOP_BYTE, eam_module_msg::temperature1, eam_module_msg::temperature2, and battery_status_s::voltage_v.

Referenced by hott_telemetry_thread_main().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ build_gam_request()

void build_gam_request ( uint8_t *  buffer,
size_t *  size 
)

Definition at line 90 of file messages.cpp.

References BINARY_MODE_REQUEST_ID, GAM_SENSOR_ID, gam_module_poll_msg::id, gam_module_poll_msg::mode, and msg.

Referenced by hott_sensors_thread_main().

Here is the caller graph for this function:

◆ build_gam_response()

void build_gam_response ( uint8_t *  buffer,
size_t *  size 
)

Definition at line 174 of file messages.cpp.

References _esc_sub, gam_module_msg::current_H, gam_module_msg::current_L, esc_status_s::esc, esc_report_s::esc_current, esc_report_s::esc_rpm, esc_report_s::esc_temperature, esc_report_s::esc_voltage, GAM_SENSOR_ID, gam_module_msg::gam_sensor_id, GAM_SENSOR_TEXT_ID, gam_module_msg::main_voltage_H, gam_module_msg::main_voltage_L, msg, orb_copy(), ORB_ID, gam_module_msg::rpm_H, gam_module_msg::rpm_L, gam_module_msg::sensor_text_id, gam_module_msg::start, START_BYTE, gam_module_msg::stop, STOP_BYTE, gam_module_msg::temperature1, and gam_module_msg::temperature2.

Referenced by hott_telemetry_thread_main().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ build_gps_response()

void build_gps_response ( uint8_t *  buffer,
size_t *  size 
)

Definition at line 209 of file messages.cpp.

References _gps_sub, _home_lat, _home_lon, _home_position_set, _home_sub, matrix::abs(), vehicle_gps_position_s::alt, gps_module_msg::altitude_H, gps_module_msg::altitude_L, vehicle_gps_position_s::cog_rad, convert_to_degrees_minutes_seconds(), gps_module_msg::distance_H, gps_module_msg::distance_L, f(), vehicle_gps_position_s::fix_type, gps_module_msg::flight_direction, get_bearing_to_next_waypoint(), get_distance_to_next_waypoint(), gps_module_msg::gps_fix, gps_module_msg::gps_fix_char, gps_module_msg::gps_num_sat, GPS_SENSOR_ID, GPS_SENSOR_TEXT_ID, gps_module_msg::gps_speed_H, gps_module_msg::gps_speed_L, gps_module_msg::home_direction, home_position_s::lat, vehicle_gps_position_s::lat, gps_module_msg::latitude_min_H, gps_module_msg::latitude_min_L, gps_module_msg::latitude_ns, gps_module_msg::latitude_sec_H, gps_module_msg::latitude_sec_L, home_position_s::lon, vehicle_gps_position_s::lon, gps_module_msg::longitude_ew, gps_module_msg::longitude_min_H, gps_module_msg::longitude_min_L, gps_module_msg::longitude_sec_H, gps_module_msg::longitude_sec_L, math::min(), msg, orb_check(), orb_copy(), ORB_ID, vehicle_gps_position_s::satellites_used, gps_module_msg::sensor_id, gps_module_msg::sensor_text_id, gps_module_msg::start, START_BYTE, gps_module_msg::stop, STOP_BYTE, and vehicle_gps_position_s::vel_m_s.

Referenced by hott_telemetry_thread_main().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ convert_to_degrees_minutes_seconds()

void convert_to_degrees_minutes_seconds ( double  val,
int *  deg,
int *  min,
int *  sec 
)

Definition at line 321 of file messages.cpp.

References math::min().

Referenced by build_gps_response().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ init_pub_messages()

void init_pub_messages ( void  )

Definition at line 85 of file messages.cpp.

Referenced by hott_sensors_thread_main().

Here is the caller graph for this function:

◆ init_sub_messages()

void init_sub_messages ( void  )

Definition at line 74 of file messages.cpp.

References _airdata_sub, _airspeed_sub, _battery_sub, _esc_sub, _gps_sub, _home_sub, ORB_ID, and orb_subscribe().

Referenced by hott_telemetry_thread_main().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publish_gam_message()

void publish_gam_message ( const uint8_t *  buffer)

Definition at line 103 of file messages.cpp.

References _esc_pub, gam_module_msg::current_H, gam_module_msg::current_L, esc_status_s::esc, esc_status_s::esc_connectiontype, esc_status_s::esc_count, esc_report_s::esc_current, esc_report_s::esc_rpm, esc_report_s::esc_temperature, esc_report_s::esc_voltage, hrt_absolute_time(), gam_module_msg::main_voltage_H, gam_module_msg::main_voltage_L, msg, orb_advertise(), ORB_ID, orb_publish(), gam_module_msg::rpm_H, gam_module_msg::rpm_L, gam_module_msg::temperature1, and esc_status_s::timestamp.

Referenced by hott_sensors_thread_main().

Here is the call graph for this function:
Here is the caller graph for this function:

Variable Documentation

◆ _airdata_sub

int _airdata_sub = -1
static

Definition at line 63 of file messages.cpp.

Referenced by build_eam_response(), and init_sub_messages().

◆ _airspeed_sub

int _airspeed_sub = -1
static

Definition at line 64 of file messages.cpp.

Referenced by build_eam_response(), and init_sub_messages().

◆ _battery_sub

int _battery_sub = -1
static

Definition at line 60 of file messages.cpp.

Referenced by build_eam_response(), and init_sub_messages().

◆ _esc_pub

orb_advert_t _esc_pub = nullptr
static

Definition at line 67 of file messages.cpp.

Referenced by publish_gam_message().

◆ _esc_sub

int _esc_sub = -1
static

Definition at line 65 of file messages.cpp.

Referenced by build_gam_response(), and init_sub_messages().

◆ _gps_sub

int _gps_sub = -1
static

Definition at line 61 of file messages.cpp.

Referenced by build_gps_response(), and init_sub_messages().

◆ _home_lat

double _home_lat = 0.0d
static

Definition at line 70 of file messages.cpp.

Referenced by build_gps_response().

◆ _home_lon

double _home_lon = 0.0d
static

Definition at line 71 of file messages.cpp.

Referenced by build_gps_response().

◆ _home_position_set

bool _home_position_set = false
static

Definition at line 69 of file messages.cpp.

Referenced by build_gps_response().

◆ _home_sub

int _home_sub = -1
static