2012-04-30 04:17:14 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-05-13 00:16:45 -03:00
# include "Rover.h"
2013-10-04 00:33:47 -03:00
// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
2014-05-15 07:52:50 -03:00
# define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
2013-10-04 00:33:47 -03:00
2015-05-12 02:03:23 -03:00
void Rover : : send_heartbeat ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED ;
uint8_t system_status = MAV_STATE_ACTIVE ;
uint32_t custom_mode = control_mode ;
2013-02-08 22:11:43 -04:00
2013-03-28 20:25:53 -03:00
if ( failsafe . triggered ! = 0 ) {
2013-02-08 22:11:43 -04:00
system_status = MAV_STATE_CRITICAL ;
}
2012-04-30 04:17:14 -03:00
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch ( control_mode ) {
case MANUAL :
2012-05-14 16:21:29 -03:00
case LEARNING :
2013-03-01 07:32:57 -04:00
case STEERING :
2012-06-10 06:34:11 -03:00
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ;
2012-04-30 04:17:14 -03:00
break ;
case AUTO :
case RTL :
case GUIDED :
2012-06-10 06:34:11 -03:00
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED ;
2012-04-30 04:17:14 -03:00
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break ;
case INITIALISING :
system_status = MAV_STATE_CALIBRATING ;
break ;
2013-03-28 18:53:20 -03:00
case HOLD :
system_status = 0 ;
break ;
2012-04-30 04:17:14 -03:00
}
2015-02-05 02:23:49 -04:00
# if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING==ENABLED)
2012-04-30 04:17:14 -03:00
if ( control_mode ! = INITIALISING ) {
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode | = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ;
}
# endif
# if HIL_MODE != HIL_MODE_DISABLED
base_mode | = MAV_MODE_FLAG_HIL_ENABLED ;
# endif
// we are armed if we are not initialising
2015-01-28 19:44:50 -04:00
if ( control_mode ! = INITIALISING & & hal . util - > get_soft_armed ( ) ) {
2012-04-30 04:17:14 -03:00
base_mode | = MAV_MODE_FLAG_SAFETY_ARMED ;
}
// indicate we have set a custom mode
base_mode | = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED ;
mavlink_msg_heartbeat_send (
chan ,
2013-03-28 18:16:29 -03:00
MAV_TYPE_GROUND_ROVER ,
2012-04-30 04:17:14 -03:00
MAV_AUTOPILOT_ARDUPILOTMEGA ,
base_mode ,
custom_mode ,
system_status ) ;
}
2015-05-12 02:03:23 -03:00
void Rover : : send_attitude ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
Vector3f omega = ahrs . get_gyro ( ) ;
mavlink_msg_attitude_send (
chan ,
2012-12-18 15:30:42 -04:00
millis ( ) ,
2012-04-30 04:17:14 -03:00
ahrs . roll ,
2012-11-17 02:45:20 -04:00
ahrs . pitch ,
ahrs . yaw ,
2012-04-30 04:17:14 -03:00
omega . x ,
omega . y ,
omega . z ) ;
}
2015-05-12 02:03:23 -03:00
void Rover : : send_extended_status1 ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2013-10-04 00:33:47 -03:00
uint32_t control_sensors_present ;
2012-04-30 04:17:14 -03:00
uint32_t control_sensors_enabled ;
uint32_t control_sensors_health ;
2013-10-04 00:33:47 -03:00
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT ;
2012-04-30 04:17:14 -03:00
// first what sensors/controllers we have
if ( g . compass_enabled ) {
2013-10-04 00:33:47 -03:00
control_sensors_present | = MAV_SYS_STATUS_SENSOR_3D_MAG ; // compass present
2012-04-30 04:17:14 -03:00
}
2014-03-30 22:01:54 -03:00
if ( gps . status ( ) > AP_GPS : : NO_GPS ) {
2013-10-04 00:33:47 -03:00
control_sensors_present | = MAV_SYS_STATUS_SENSOR_GPS ;
2012-04-30 04:17:14 -03:00
}
2013-10-04 00:33:47 -03:00
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
control_sensors_enabled = control_sensors_present & ( ~ MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~ MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~ MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~ MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS ) ;
2012-04-30 04:17:14 -03:00
switch ( control_mode ) {
case MANUAL :
2013-03-28 18:53:20 -03:00
case HOLD :
2012-04-30 04:17:14 -03:00
break ;
2012-05-14 16:21:29 -03:00
case LEARNING :
2013-03-01 07:32:57 -04:00
case STEERING :
2013-10-04 00:33:47 -03:00
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL ; // 3D angular rate control
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION ; // attitude stabilisation
2012-04-30 04:17:14 -03:00
break ;
case AUTO :
case RTL :
2013-02-07 18:21:22 -04:00
case GUIDED :
2013-10-04 00:33:47 -03:00
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL ; // 3D angular rate control
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION ; // attitude stabilisation
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_YAW_POSITION ; // yaw position
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL ; // X/Y position control
2012-04-30 04:17:14 -03:00
break ;
case INITIALISING :
break ;
}
2014-09-14 05:38:03 -03:00
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if ( hal . util - > safety_switch_state ( ) ! = AP_HAL : : Util : : SAFETY_DISARMED ) {
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS ;
}
2013-10-04 00:33:47 -03:00
// default to all healthy except compass and gps which we set individually
control_sensors_health = control_sensors_present & ( ~ MAV_SYS_STATUS_SENSOR_3D_MAG & ~ MAV_SYS_STATUS_SENSOR_GPS ) ;
2014-03-23 17:03:35 -03:00
if ( g . compass_enabled & & compass . healthy ( 0 ) & & ahrs . use_compass ( ) ) {
2013-10-04 00:33:47 -03:00
control_sensors_health | = MAV_SYS_STATUS_SENSOR_3D_MAG ;
2012-12-18 15:30:42 -04:00
}
2014-03-30 22:01:54 -03:00
if ( gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
2013-10-04 00:33:47 -03:00
control_sensors_health | = MAV_SYS_STATUS_SENSOR_GPS ;
2012-12-18 15:30:42 -04:00
}
2015-09-17 09:33:06 -03:00
if ( ! ins . get_gyro_health_all ( ) | | ! ins . gyro_calibrated_ok_all ( ) ) {
2014-09-01 08:22:37 -03:00
control_sensors_health & = ~ MAV_SYS_STATUS_SENSOR_3D_GYRO ;
}
if ( ! ins . get_accel_health_all ( ) ) {
control_sensors_health & = ~ MAV_SYS_STATUS_SENSOR_3D_ACCEL ;
2013-10-29 19:02:16 -03:00
}
2012-12-18 15:30:42 -04:00
2014-10-02 01:44:52 -03:00
if ( ahrs . initialised ( ) & & ! ahrs . healthy ( ) ) {
2014-05-15 07:52:50 -03:00
// AHRS subsystem is unhealthy
control_sensors_health & = ~ MAV_SYS_STATUS_AHRS ;
}
2013-10-02 03:07:28 -03:00
int16_t battery_current = - 1 ;
int8_t battery_remaining = - 1 ;
2012-04-30 04:17:14 -03:00
2015-03-18 08:12:11 -03:00
if ( battery . has_current ( ) & & battery . healthy ( ) ) {
2013-10-02 03:07:28 -03:00
battery_remaining = battery . capacity_remaining_pct ( ) ;
battery_current = battery . current_amps ( ) * 100 ;
2012-04-30 04:17:14 -03:00
}
2015-08-11 23:42:35 -03:00
if ( sonar . num_sensors ( ) > 0 ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
if ( g . sonar_trigger_cm > 0 ) {
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
}
if ( sonar . has_data ( ) ) {
control_sensors_health | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
}
}
2015-03-08 18:25:44 -03:00
if ( AP_Notify : : flags . initialising ) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled & = ~ ( MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL ) ;
control_sensors_health & = ~ ( MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL ) ;
}
2012-04-30 04:17:14 -03:00
mavlink_msg_sys_status_send (
chan ,
control_sensors_present ,
control_sensors_enabled ,
control_sensors_health ,
2013-07-23 04:07:35 -03:00
( uint16_t ) ( scheduler . load_average ( 20000 ) * 1000 ) ,
2013-10-02 03:07:28 -03:00
battery . voltage ( ) * 1000 , // mV
2012-04-30 04:17:14 -03:00
battery_current , // in 10mA units
battery_remaining , // in %
0 , // comm drops %,
0 , // comm drops in pkts,
0 , 0 , 0 , 0 ) ;
}
2015-05-12 04:00:25 -03:00
void Rover : : send_location ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2012-12-18 15:30:42 -04:00
uint32_t fix_time ;
// if we have a GPS fix, take the time as the last fix time. That
// allows us to correctly calculate velocities and extrapolate
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the fix time.
2014-03-30 22:01:54 -03:00
if ( gps . status ( ) > = AP_GPS : : GPS_OK_FIX_2D ) {
fix_time = gps . last_fix_time_ms ( ) ;
2012-12-18 15:30:42 -04:00
} else {
fix_time = millis ( ) ;
}
2014-03-30 22:01:54 -03:00
const Vector3f & vel = gps . velocity ( ) ;
2012-04-30 04:17:14 -03:00
mavlink_msg_global_position_int_send (
chan ,
2012-12-18 15:30:42 -04:00
fix_time ,
2014-03-30 22:01:54 -03:00
current_loc . lat , // in 1E7 degrees
current_loc . lng , // in 1E7 degrees
2015-09-07 08:09:31 -03:00
current_loc . alt * 10UL , // millimeters above sea level
2014-03-30 22:01:54 -03:00
( current_loc . alt - home . alt ) * 10 , // millimeters above ground
vel . x * 100 , // X speed cm/s (+ve North)
vel . y * 100 , // Y speed cm/s (+ve East)
vel . z * - 100 , // Z speed cm/s (+ve up)
2012-12-18 15:30:42 -04:00
ahrs . yaw_sensor ) ;
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
void Rover : : send_nav_controller_output ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
mavlink_msg_nav_controller_output_send (
chan ,
2013-08-25 19:46:22 -03:00
lateral_acceleration , // use nav_roll to hold demanded Y accel
2014-03-30 22:01:54 -03:00
gps . ground_speed ( ) * ins . get_gyro ( ) . z , // use nav_pitch to hold actual Y accel
2013-06-16 20:50:53 -03:00
nav_controller - > nav_bearing_cd ( ) * 0.01f ,
nav_controller - > target_bearing_cd ( ) * 0.01f ,
2012-04-30 04:17:14 -03:00
wp_distance ,
2013-09-08 21:18:31 -03:00
0 ,
2012-05-09 02:12:26 -03:00
groundspeed_error ,
2013-06-16 20:50:53 -03:00
nav_controller - > crosstrack_error ( ) ) ;
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
void Rover : : send_servo_out ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2015-02-05 02:23:49 -04:00
# if HIL_MODE != HIL_MODE_DISABLED
2012-04-30 04:17:14 -03:00
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with
// HIL maintainers
mavlink_msg_rc_channels_scaled_send (
chan ,
millis ( ) ,
0 , // port 0
2013-06-03 06:33:59 -03:00
10000 * channel_steer - > norm_output ( ) ,
2013-02-07 18:21:22 -04:00
0 ,
2013-06-03 06:33:59 -03:00
10000 * channel_throttle - > norm_output ( ) ,
2013-02-07 18:21:22 -04:00
0 ,
2012-04-30 04:17:14 -03:00
0 ,
0 ,
0 ,
0 ,
2012-12-18 15:30:42 -04:00
receiver_rssi ) ;
2013-09-20 20:27:38 -03:00
# endif
2015-02-05 02:23:49 -04:00
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
void Rover : : send_radio_out ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2012-12-18 07:44:12 -04:00
# if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
mavlink_msg_servo_output_raw_send (
chan ,
micros ( ) ,
0 , // port
hal . rcout - > read ( 0 ) ,
hal . rcout - > read ( 1 ) ,
hal . rcout - > read ( 2 ) ,
hal . rcout - > read ( 3 ) ,
hal . rcout - > read ( 4 ) ,
hal . rcout - > read ( 5 ) ,
hal . rcout - > read ( 6 ) ,
hal . rcout - > read ( 7 ) ) ;
# else
mavlink_msg_servo_output_raw_send (
chan ,
micros ( ) ,
0 , // port
2013-06-03 02:12:11 -03:00
RC_Channel : : rc_channel ( 0 ) - > radio_out ,
RC_Channel : : rc_channel ( 1 ) - > radio_out ,
RC_Channel : : rc_channel ( 2 ) - > radio_out ,
RC_Channel : : rc_channel ( 3 ) - > radio_out ,
RC_Channel : : rc_channel ( 4 ) - > radio_out ,
RC_Channel : : rc_channel ( 5 ) - > radio_out ,
RC_Channel : : rc_channel ( 6 ) - > radio_out ,
RC_Channel : : rc_channel ( 7 ) - > radio_out ) ;
2012-12-18 07:44:12 -04:00
# endif
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
void Rover : : send_vfr_hud ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
mavlink_msg_vfr_hud_send (
chan ,
2014-03-30 22:01:54 -03:00
gps . ground_speed ( ) ,
gps . ground_speed ( ) ,
2012-11-17 02:45:20 -04:00
( ahrs . yaw_sensor / 100 ) % 360 ,
2013-11-24 20:50:50 -04:00
( uint16_t ) ( 100 * fabsf ( channel_throttle - > norm_output ( ) ) ) ,
2012-04-30 04:17:14 -03:00
current_loc . alt / 100.0 ,
0 ) ;
}
2012-05-14 15:33:03 -03:00
// report simulator state
2015-05-12 04:00:25 -03:00
void Rover : : send_simstate ( mavlink_channel_t chan )
2012-05-14 15:33:03 -03:00
{
2015-05-04 03:18:29 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2012-11-17 02:45:20 -04:00
sitl . simstate_send ( chan ) ;
2012-05-14 15:33:03 -03:00
# endif
2013-04-19 18:29:57 -03:00
}
2012-05-14 15:33:03 -03:00
2015-05-12 04:00:25 -03:00
void Rover : : send_hwstatus ( mavlink_channel_t chan )
2012-05-14 15:33:03 -03:00
{
mavlink_msg_hwstatus_send (
chan ,
2014-02-13 02:11:57 -04:00
hal . analogin - > board_voltage ( ) * 1000 ,
2012-12-18 07:44:12 -04:00
hal . i2c - > lockup_count ( ) ) ;
2012-05-14 15:33:03 -03:00
}
2015-05-12 04:00:25 -03:00
void Rover : : send_rangefinder ( mavlink_channel_t chan )
2013-02-28 21:00:48 -04:00
{
2015-04-17 03:41:00 -03:00
if ( ! sonar . has_data ( 0 ) & & ! sonar . has_data ( 1 ) ) {
2013-03-28 21:00:41 -03:00
// no sonar to report
return ;
}
2015-04-17 03:41:00 -03:00
float distance_cm = 0.0f ;
float voltage = 0.0f ;
2013-03-28 21:00:41 -03:00
/*
2015-04-17 03:41:00 -03:00
report smaller distance of two sonars
2013-03-28 21:00:41 -03:00
*/
2015-04-17 03:41:00 -03:00
if ( sonar . has_data ( 0 ) & & sonar . has_data ( 1 ) ) {
if ( sonar . distance_cm ( 0 ) < = sonar . distance_cm ( 1 ) ) {
distance_cm = sonar . distance_cm ( 0 ) ;
voltage = sonar . voltage_mv ( 0 ) ;
} else {
distance_cm = sonar . distance_cm ( 1 ) ;
voltage = sonar . voltage_mv ( 1 ) ;
}
2013-03-28 21:00:41 -03:00
} else {
2015-04-17 03:41:00 -03:00
// only sonar 0 or sonar 1 has data
if ( sonar . has_data ( 0 ) ) {
distance_cm = sonar . distance_cm ( 0 ) ;
2014-06-27 00:18:20 -03:00
voltage = sonar . voltage_mv ( 0 ) * 0.001f ;
2015-04-17 03:41:00 -03:00
}
if ( sonar . has_data ( 1 ) ) {
distance_cm = sonar . distance_cm ( 1 ) ;
2014-06-27 00:18:20 -03:00
voltage = sonar . voltage_mv ( 1 ) * 0.001f ;
2013-03-28 21:00:41 -03:00
}
}
2015-04-17 03:41:00 -03:00
2013-02-28 21:00:48 -04:00
mavlink_msg_rangefinder_send (
chan ,
2013-03-28 21:00:41 -03:00
distance_cm * 0.01f ,
voltage ) ;
2013-02-28 21:00:48 -04:00
}
2015-06-18 04:37:59 -03:00
/*
send PID tuning message
*/
void Rover : : send_pid_tuning ( mavlink_channel_t chan )
{
const Vector3f & gyro = ahrs . get_gyro ( ) ;
if ( g . gcs_pid_mask & 1 ) {
const DataFlash_Class : : PID_Info & pid_info = steerController . get_pid_info ( ) ;
mavlink_msg_pid_tuning_send ( chan , PID_TUNING_STEER ,
pid_info . desired ,
degrees ( gyro . z ) ,
pid_info . FF ,
pid_info . P ,
pid_info . I ,
pid_info . D ) ;
if ( ! HAVE_PAYLOAD_SPACE ( chan , PID_TUNING ) ) {
return ;
}
}
}
2015-05-12 04:00:25 -03:00
void Rover : : send_current_waypoint ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2014-04-21 22:38:10 -03:00
mavlink_msg_mission_current_send ( chan , mission . get_current_nav_index ( ) ) ;
2012-04-30 04:17:14 -03:00
}
2015-05-12 04:00:25 -03:00
void Rover : : send_statustext ( mavlink_channel_t chan )
2012-04-30 04:17:14 -03:00
{
2013-11-23 06:57:26 -04:00
mavlink_statustext_t * s = & gcs [ chan - MAVLINK_COMM_0 ] . pending_status ;
2012-04-30 04:17:14 -03:00
mavlink_msg_statustext_send (
chan ,
2012-12-18 07:44:12 -04:00
s - > severity ,
s - > text ) ;
2012-04-30 04:17:14 -03:00
}
2012-08-29 20:36:18 -03:00
// are we still delaying telemetry to try to avoid Xbee bricking?
2015-05-12 02:03:23 -03:00
bool Rover : : telemetry_delayed ( mavlink_channel_t chan )
2012-08-29 20:36:18 -03:00
{
uint32_t tnow = millis ( ) > > 10 ;
2013-01-13 05:05:14 -04:00
if ( tnow > ( uint32_t ) g . telem_delay ) {
2012-08-29 20:36:18 -03:00
return false ;
}
2013-09-19 03:24:59 -03:00
if ( chan = = MAVLINK_COMM_0 & & hal . gpio - > usb_connected ( ) ) {
// this is USB telemetry, so won't be an Xbee
2012-08-29 20:36:18 -03:00
return false ;
}
// we're either on the 2nd UART, or no USB cable is connected
2013-09-19 03:24:59 -03:00
// we need to delay telemetry by the TELEM_DELAY time
2012-08-29 20:36:18 -03:00
return true ;
}
2012-04-30 04:17:14 -03:00
// try to send a message, return false if it won't fit in the serial tx buffer
2014-03-18 21:07:15 -03:00
bool GCS_MAVLINK : : try_send_message ( enum ap_message id )
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
if ( rover . telemetry_delayed ( chan ) ) {
2012-04-30 04:17:14 -03:00
return false ;
}
2013-10-27 20:33:52 -03:00
// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
2015-05-12 04:00:25 -03:00
if ( ! rover . in_mavlink_delay & & rover . scheduler . time_available_usec ( ) < 1200 ) {
rover . gcs_out_of_time = true ;
2013-10-27 20:33:52 -03:00
return false ;
}
2012-04-30 04:17:14 -03:00
switch ( id ) {
case MSG_HEARTBEAT :
CHECK_PAYLOAD_SIZE ( HEARTBEAT ) ;
2015-11-19 23:04:16 -04:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . last_heartbeat_time = AP_HAL : : millis ( ) ;
2015-05-12 04:00:25 -03:00
rover . send_heartbeat ( chan ) ;
2012-04-30 04:17:14 -03:00
return true ;
case MSG_EXTENDED_STATUS1 :
CHECK_PAYLOAD_SIZE ( SYS_STATUS ) ;
2015-05-12 04:00:25 -03:00
rover . send_extended_status1 ( chan ) ;
2014-02-13 07:10:11 -04:00
CHECK_PAYLOAD_SIZE ( POWER_STATUS ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_power_status ( ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_EXTENDED_STATUS2 :
CHECK_PAYLOAD_SIZE ( MEMINFO ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_meminfo ( ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_ATTITUDE :
CHECK_PAYLOAD_SIZE ( ATTITUDE ) ;
2015-05-12 04:00:25 -03:00
rover . send_attitude ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_LOCATION :
CHECK_PAYLOAD_SIZE ( GLOBAL_POSITION_INT ) ;
2015-05-12 04:00:25 -03:00
rover . send_location ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
2015-04-05 13:25:28 -03:00
case MSG_LOCAL_POSITION :
CHECK_PAYLOAD_SIZE ( LOCAL_POSITION_NED ) ;
2015-05-12 04:00:25 -03:00
send_local_position ( rover . ahrs ) ;
2015-04-05 13:25:28 -03:00
break ;
2012-04-30 04:17:14 -03:00
case MSG_NAV_CONTROLLER_OUTPUT :
2015-05-12 04:00:25 -03:00
if ( rover . control_mode ! = MANUAL ) {
2012-04-30 04:17:14 -03:00
CHECK_PAYLOAD_SIZE ( NAV_CONTROLLER_OUTPUT ) ;
2015-05-12 04:00:25 -03:00
rover . send_nav_controller_output ( chan ) ;
2012-04-30 04:17:14 -03:00
}
break ;
case MSG_GPS_RAW :
CHECK_PAYLOAD_SIZE ( GPS_RAW_INT ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_gps_raw ( rover . gps ) ;
2012-04-30 04:17:14 -03:00
break ;
2013-10-23 08:15:37 -03:00
case MSG_SYSTEM_TIME :
CHECK_PAYLOAD_SIZE ( SYSTEM_TIME ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_system_time ( rover . gps ) ;
2013-10-23 08:15:37 -03:00
break ;
2012-04-30 04:17:14 -03:00
case MSG_SERVO_OUT :
CHECK_PAYLOAD_SIZE ( RC_CHANNELS_SCALED ) ;
2015-05-12 04:00:25 -03:00
rover . send_servo_out ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_RADIO_IN :
CHECK_PAYLOAD_SIZE ( RC_CHANNELS_RAW ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_radio_in ( rover . receiver_rssi ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_RADIO_OUT :
CHECK_PAYLOAD_SIZE ( SERVO_OUTPUT_RAW ) ;
2015-05-12 04:00:25 -03:00
rover . send_radio_out ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_VFR_HUD :
CHECK_PAYLOAD_SIZE ( VFR_HUD ) ;
2015-05-12 04:00:25 -03:00
rover . send_vfr_hud ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_RAW_IMU1 :
CHECK_PAYLOAD_SIZE ( RAW_IMU ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_raw_imu ( rover . ins , rover . compass ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_RAW_IMU3 :
CHECK_PAYLOAD_SIZE ( SENSOR_OFFSETS ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_sensor_offsets ( rover . ins , rover . compass , rover . barometer ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_CURRENT_WAYPOINT :
2012-08-08 23:22:46 -03:00
CHECK_PAYLOAD_SIZE ( MISSION_CURRENT ) ;
2015-05-12 04:00:25 -03:00
rover . send_current_waypoint ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_NEXT_PARAM :
CHECK_PAYLOAD_SIZE ( PARAM_VALUE ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . queued_param_send ( ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_NEXT_WAYPOINT :
2012-08-08 23:22:46 -03:00
CHECK_PAYLOAD_SIZE ( MISSION_REQUEST ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . queued_waypoint_send ( ) ;
2012-04-30 04:17:14 -03:00
break ;
case MSG_STATUSTEXT :
CHECK_PAYLOAD_SIZE ( STATUSTEXT ) ;
2015-05-12 04:00:25 -03:00
rover . send_statustext ( chan ) ;
2012-04-30 04:17:14 -03:00
break ;
2012-05-14 15:33:03 -03:00
case MSG_AHRS :
CHECK_PAYLOAD_SIZE ( AHRS ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_ahrs ( rover . ahrs ) ;
2012-05-14 15:33:03 -03:00
break ;
case MSG_SIMSTATE :
CHECK_PAYLOAD_SIZE ( SIMSTATE ) ;
2015-05-12 04:00:25 -03:00
rover . send_simstate ( chan ) ;
2012-05-14 15:33:03 -03:00
break ;
case MSG_HWSTATUS :
CHECK_PAYLOAD_SIZE ( HWSTATUS ) ;
2015-05-12 04:00:25 -03:00
rover . send_hwstatus ( chan ) ;
2012-05-14 15:33:03 -03:00
break ;
2013-02-28 21:00:48 -04:00
case MSG_RANGEFINDER :
CHECK_PAYLOAD_SIZE ( RANGEFINDER ) ;
2015-05-12 04:00:25 -03:00
rover . send_rangefinder ( chan ) ;
2013-02-28 21:00:48 -04:00
break ;
2014-11-17 19:43:11 -04:00
case MSG_MOUNT_STATUS :
# if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE ( MOUNT_STATUS ) ;
2015-05-12 04:00:25 -03:00
rover . camera_mount . status_msg ( chan ) ;
2014-11-17 19:43:11 -04:00
# endif // MOUNT == ENABLED
break ;
2013-12-15 19:35:27 -04:00
case MSG_RAW_IMU2 :
case MSG_LIMITS_STATUS :
case MSG_FENCE_STATUS :
case MSG_WIND :
2015-06-12 04:02:59 -03:00
case MSG_VIBRATION :
2013-12-15 19:35:27 -04:00
// unused
break ;
2015-01-06 00:17:56 -04:00
case MSG_BATTERY2 :
CHECK_PAYLOAD_SIZE ( BATTERY2 ) ;
2015-05-12 04:00:25 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_battery2 ( rover . battery ) ;
2015-01-06 00:17:56 -04:00
break ;
2015-01-06 00:28:38 -04:00
case MSG_CAMERA_FEEDBACK :
# if CAMERA == ENABLED
CHECK_PAYLOAD_SIZE ( CAMERA_FEEDBACK ) ;
2015-05-12 04:00:25 -03:00
rover . camera . send_feedback ( chan , rover . gps , rover . ahrs , rover . current_loc ) ;
2015-01-06 00:28:38 -04:00
# endif
break ;
2015-03-12 00:31:45 -03:00
case MSG_EKF_STATUS_REPORT :
# if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE ( EKF_STATUS_REPORT ) ;
2015-09-28 21:59:28 -03:00
rover . ahrs . send_ekf_status_report ( chan ) ;
2015-03-12 00:31:45 -03:00
# endif
break ;
2015-06-18 04:37:59 -03:00
case MSG_PID_TUNING :
CHECK_PAYLOAD_SIZE ( PID_TUNING ) ;
rover . send_pid_tuning ( chan ) ;
break ;
2015-07-24 04:31:30 -03:00
case MSG_MISSION_ITEM_REACHED :
CHECK_PAYLOAD_SIZE ( MISSION_ITEM_REACHED ) ;
mavlink_msg_mission_item_reached_send ( chan , mission_item_reached_index ) ;
break ;
2015-07-30 22:24:32 -03:00
case MSG_MAG_CAL_PROGRESS :
CHECK_PAYLOAD_SIZE ( MAG_CAL_PROGRESS ) ;
rover . compass . send_mag_cal_progress ( chan ) ;
break ;
case MSG_MAG_CAL_REPORT :
CHECK_PAYLOAD_SIZE ( MAG_CAL_REPORT ) ;
rover . compass . send_mag_cal_report ( chan ) ;
break ;
2015-01-06 00:28:38 -04:00
case MSG_RETRY_DEFERRED :
case MSG_TERRAIN :
case MSG_OPTICAL_FLOW :
2015-01-29 06:50:03 -04:00
case MSG_GIMBAL_REPORT :
2015-08-10 01:27:38 -03:00
case MSG_RPM :
2015-01-06 00:28:38 -04:00
break ; // just here to prevent a warning
2015-07-30 22:24:32 -03:00
}
2013-12-15 19:35:27 -04:00
2012-04-30 04:17:14 -03:00
return true ;
}
2013-10-20 19:32:39 -03:00
/*
default stream rates to 1 Hz
*/
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo GCS_MAVLINK : : var_info [ ] = {
2013-09-11 20:51:36 -03:00
// @Param: RAW_SENS
2013-10-20 19:32:39 -03:00
// @DisplayName: Raw sensor stream rate
// @Description: Raw sensor stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " RAW_SENS " , 0 , GCS_MAVLINK , streamRates [ 0 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: EXT_STAT
2013-10-20 19:32:39 -03:00
// @DisplayName: Extended status stream rate to ground station
// @Description: Extended status stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " EXT_STAT " , 1 , GCS_MAVLINK , streamRates [ 1 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: RC_CHAN
2013-10-20 19:32:39 -03:00
// @DisplayName: RC Channel stream rate to ground station
// @Description: RC Channel stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " RC_CHAN " , 2 , GCS_MAVLINK , streamRates [ 2 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: RAW_CTRL
2013-10-20 19:32:39 -03:00
// @DisplayName: Raw Control stream rate to ground station
// @Description: Raw Control stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " RAW_CTRL " , 3 , GCS_MAVLINK , streamRates [ 3 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: POSITION
2013-10-20 19:32:39 -03:00
// @DisplayName: Position stream rate to ground station
// @Description: Position stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " POSITION " , 4 , GCS_MAVLINK , streamRates [ 4 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: EXTRA1
2013-10-20 19:32:39 -03:00
// @DisplayName: Extra data type 1 stream rate to ground station
// @Description: Extra data type 1 stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " EXTRA1 " , 5 , GCS_MAVLINK , streamRates [ 5 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: EXTRA2
2013-10-20 19:32:39 -03:00
// @DisplayName: Extra data type 2 stream rate to ground station
// @Description: Extra data type 2 stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " EXTRA2 " , 6 , GCS_MAVLINK , streamRates [ 6 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: EXTRA3
2013-10-20 19:32:39 -03:00
// @DisplayName: Extra data type 3 stream rate to ground station
// @Description: Extra data type 3 stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-20 19:32:39 -03:00
AP_GROUPINFO ( " EXTRA3 " , 7 , GCS_MAVLINK , streamRates [ 7 ] , 1 ) ,
2013-09-11 20:51:36 -03:00
// @Param: PARAMS
2013-10-20 19:32:39 -03:00
// @DisplayName: Parameter stream rate to ground station
// @Description: Parameter stream rate to ground station
2013-09-11 20:51:36 -03:00
// @Units: Hz
2013-10-20 19:32:39 -03:00
// @Range: 0 10
2013-09-11 20:51:36 -03:00
// @Increment: 1
// @User: Advanced
2013-10-27 20:33:52 -03:00
AP_GROUPINFO ( " PARAMS " , 8 , GCS_MAVLINK , streamRates [ 8 ] , 10 ) ,
2012-04-30 04:17:14 -03:00
AP_GROUPEND
} ;
2012-05-14 15:33:03 -03:00
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK : : stream_trigger ( enum streams stream_num )
{
2013-10-20 19:32:39 -03:00
if ( stream_num > = NUM_STREAMS ) {
return false ;
}
float rate = ( uint8_t ) streamRates [ stream_num ] . get ( ) ;
2012-05-14 15:33:03 -03:00
2012-12-18 15:30:42 -04:00
// send at a much lower rate while handling waypoints and
// parameter sends
2013-10-27 20:33:52 -03:00
if ( ( stream_num ! = STREAM_PARAMS ) & &
( waypoint_receiving | | _queued_parameter ! = NULL ) ) {
2015-05-03 03:01:05 -03:00
rate * = 0.25f ;
2012-12-18 15:30:42 -04:00
}
if ( rate < = 0 ) {
2012-05-14 15:33:03 -03:00
return false ;
}
if ( stream_ticks [ stream_num ] = = 0 ) {
// we're triggering now, setup the next trigger point
if ( rate > 50 ) {
rate = 50 ;
}
2015-02-19 09:50:42 -04:00
stream_ticks [ stream_num ] = ( 50 / rate ) - 1 + stream_slowdown ;
2012-05-14 15:33:03 -03:00
return true ;
}
// count down at 50Hz
stream_ticks [ stream_num ] - - ;
return false ;
}
2012-04-30 04:17:14 -03:00
void
2012-05-14 15:33:03 -03:00
GCS_MAVLINK : : data_stream_send ( void )
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
rover . gcs_out_of_time = false ;
2013-10-27 20:33:52 -03:00
2015-05-12 04:00:25 -03:00
if ( ! rover . in_mavlink_delay ) {
handle_log_send ( rover . DataFlash ) ;
2013-12-15 19:35:27 -04:00
}
2012-05-14 15:33:03 -03:00
if ( _queued_parameter ! = NULL ) {
2013-10-20 19:32:39 -03:00
if ( streamRates [ STREAM_PARAMS ] . get ( ) < = 0 ) {
2013-10-27 20:33:52 -03:00
streamRates [ STREAM_PARAMS ] . set ( 10 ) ;
2012-05-14 15:33:03 -03:00
}
if ( stream_trigger ( STREAM_PARAMS ) ) {
send_message ( MSG_NEXT_PARAM ) ;
}
2012-12-18 15:30:42 -04:00
}
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . in_mavlink_delay ) {
2012-12-18 15:30:42 -04:00
# if HIL_MODE != HIL_MODE_DISABLED
// in HIL we need to keep sending servo values to ensure
// the simulator doesn't pause, otherwise our sensor
// calibration could stall
if ( stream_trigger ( STREAM_RAW_CONTROLLER ) ) {
send_message ( MSG_SERVO_OUT ) ;
}
2012-12-18 16:17:06 -04:00
if ( stream_trigger ( STREAM_RC_CHANNELS ) ) {
send_message ( MSG_RADIO_OUT ) ;
}
2012-12-18 15:30:42 -04:00
# endif
2012-12-18 16:17:06 -04:00
// don't send any other stream types while in the delay callback
2012-05-14 15:33:03 -03:00
return ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_RAW_SENSORS ) ) {
send_message ( MSG_RAW_IMU1 ) ;
send_message ( MSG_RAW_IMU3 ) ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_EXTENDED_STATUS ) ) {
send_message ( MSG_EXTENDED_STATUS1 ) ;
send_message ( MSG_EXTENDED_STATUS2 ) ;
send_message ( MSG_CURRENT_WAYPOINT ) ;
send_message ( MSG_GPS_RAW ) ; // TODO - remove this message after location message is working
send_message ( MSG_NAV_CONTROLLER_OUTPUT ) ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_POSITION ) ) {
// sent with GPS read
send_message ( MSG_LOCATION ) ;
2015-04-05 13:25:28 -03:00
send_message ( MSG_LOCAL_POSITION ) ;
2012-05-14 15:33:03 -03:00
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_RAW_CONTROLLER ) ) {
send_message ( MSG_SERVO_OUT ) ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_RC_CHANNELS ) ) {
send_message ( MSG_RADIO_OUT ) ;
send_message ( MSG_RADIO_IN ) ;
}
2012-04-30 04:17:14 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_EXTRA1 ) ) {
send_message ( MSG_ATTITUDE ) ;
send_message ( MSG_SIMSTATE ) ;
2015-06-18 04:37:59 -03:00
if ( rover . control_mode ! = MANUAL ) {
send_message ( MSG_PID_TUNING ) ;
}
2012-05-14 15:33:03 -03:00
}
2015-06-18 04:37:59 -03:00
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_EXTRA2 ) ) {
send_message ( MSG_VFR_HUD ) ;
}
2015-05-12 04:00:25 -03:00
if ( rover . gcs_out_of_time ) return ;
2013-10-27 20:33:52 -03:00
2012-05-14 15:33:03 -03:00
if ( stream_trigger ( STREAM_EXTRA3 ) ) {
send_message ( MSG_AHRS ) ;
send_message ( MSG_HWSTATUS ) ;
2013-02-28 21:00:48 -04:00
send_message ( MSG_RANGEFINDER ) ;
2013-10-23 08:15:37 -03:00
send_message ( MSG_SYSTEM_TIME ) ;
2015-01-06 00:17:56 -04:00
send_message ( MSG_BATTERY2 ) ;
2015-07-30 22:24:32 -03:00
send_message ( MSG_MAG_CAL_REPORT ) ;
send_message ( MSG_MAG_CAL_PROGRESS ) ;
2014-11-17 19:43:11 -04:00
send_message ( MSG_MOUNT_STATUS ) ;
2015-03-12 00:31:45 -03:00
send_message ( MSG_EKF_STATUS_REPORT ) ;
2012-05-14 15:33:03 -03:00
}
2012-04-30 04:17:14 -03:00
}
2014-03-18 20:06:58 -03:00
void GCS_MAVLINK : : handle_guided_request ( AP_Mission : : Mission_Command & cmd )
2012-04-30 04:17:14 -03:00
{
2015-06-03 09:31:31 -03:00
if ( rover . control_mode ! = GUIDED ) {
// only accept position updates when in GUIDED mode
return ;
}
2015-05-12 04:00:25 -03:00
rover . guided_WP = cmd . content . location ;
2014-03-18 20:06:58 -03:00
// make any new wp uploaded instant (in case we are already in Guided mode)
2015-05-12 04:00:25 -03:00
rover . rtl_complete = false ;
rover . set_guided_WP ( ) ;
2014-03-18 20:06:58 -03:00
}
void GCS_MAVLINK : : handle_change_alt_request ( AP_Mission : : Mission_Command & cmd )
{
// nothing to do
}
2012-04-30 04:17:14 -03:00
2014-03-18 20:06:58 -03:00
void GCS_MAVLINK : : handleMessage ( mavlink_message_t * msg )
{
2012-04-30 04:17:14 -03:00
switch ( msg - > msgid ) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM :
{
2014-03-18 18:54:16 -03:00
handle_request_data_stream ( msg , true ) ;
2012-04-30 04:17:14 -03:00
break ;
}
case MAVLINK_MSG_ID_COMMAND_LONG :
{
// decode
mavlink_command_long_t packet ;
mavlink_msg_command_long_decode ( msg , & packet ) ;
2014-03-17 04:07:18 -03:00
uint8_t result = MAV_RESULT_UNSUPPORTED ;
2012-04-30 04:17:14 -03:00
// do command
2015-11-18 14:53:14 -04:00
send_text ( MAV_SEVERITY_INFO , " Command received: " ) ;
2012-04-30 04:17:14 -03:00
switch ( packet . command ) {
2015-06-10 03:45:56 -03:00
case MAV_CMD_START_RX_PAIR :
// initiate bind procedure
if ( ! hal . rcin - > rc_bind ( packet . param1 ) ) {
result = MAV_RESULT_FAILED ;
} else {
result = MAV_RESULT_ACCEPTED ;
}
break ;
2012-04-30 04:17:14 -03:00
case MAV_CMD_NAV_RETURN_TO_LAUNCH :
2015-05-12 04:00:25 -03:00
rover . set_mode ( RTL ) ;
2012-04-30 04:17:14 -03:00
result = MAV_RESULT_ACCEPTED ;
break ;
2014-07-04 03:54:15 -03:00
# if MOUNT == ENABLED
// Sets the region of interest (ROI) for the camera
case MAV_CMD_DO_SET_ROI :
2015-08-27 02:36:15 -03:00
// sanity check location
if ( fabsf ( packet . param5 ) > 90.0f | | fabsf ( packet . param6 ) > 180.0f ) {
break ;
}
2014-07-04 03:54:15 -03:00
Location roi_loc ;
roi_loc . lat = ( int32_t ) ( packet . param5 * 1.0e7 f ) ;
roi_loc . lng = ( int32_t ) ( packet . param6 * 1.0e7 f ) ;
roi_loc . alt = ( int32_t ) ( packet . param7 * 100.0f ) ;
if ( roi_loc . lat = = 0 & & roi_loc . lng = = 0 & & roi_loc . alt = = 0 ) {
// switch off the camera tracking if enabled
2015-05-12 04:00:25 -03:00
if ( rover . camera_mount . get_mode ( ) = = MAV_MOUNT_MODE_GPS_POINT ) {
rover . camera_mount . set_mode_to_default ( ) ;
2014-07-04 03:54:15 -03:00
}
} else {
// send the command to the camera mount
2015-05-12 04:00:25 -03:00
rover . camera_mount . set_roi_target ( roi_loc ) ;
2014-07-04 03:54:15 -03:00
}
result = MAV_RESULT_ACCEPTED ;
break ;
# endif
2015-09-06 17:44:51 -03:00
# if CAMERA == ENABLED
case MAV_CMD_DO_DIGICAM_CONFIGURE :
rover . camera . configure ( packet . param1 ,
packet . param2 ,
packet . param3 ,
packet . param4 ,
packet . param5 ,
packet . param6 ,
packet . param7 ) ;
result = MAV_RESULT_ACCEPTED ;
break ;
case MAV_CMD_DO_DIGICAM_CONTROL :
2016-01-28 18:32:15 -04:00
if ( rover . camera . control ( packet . param1 ,
packet . param2 ,
packet . param3 ,
packet . param4 ,
packet . param5 ,
packet . param6 ) ) {
rover . log_picture ( ) ;
}
2015-09-06 17:44:51 -03:00
result = MAV_RESULT_ACCEPTED ;
break ;
# endif // CAMERA == ENABLED
2015-09-02 04:46:03 -03:00
case MAV_CMD_DO_MOUNT_CONTROL :
# if MOUNT == ENABLED
rover . camera_mount . control ( packet . param1 , packet . param2 , packet . param3 , ( MAV_MOUNT_MODE ) packet . param7 ) ;
# endif
break ;
2012-05-14 15:33:03 -03:00
case MAV_CMD_MISSION_START :
2015-05-12 04:00:25 -03:00
rover . set_mode ( AUTO ) ;
2012-05-14 15:33:03 -03:00
result = MAV_RESULT_ACCEPTED ;
2012-04-30 04:17:14 -03:00
break ;
case MAV_CMD_PREFLIGHT_CALIBRATION :
2015-10-21 18:22:45 -03:00
if ( hal . util - > get_soft_armed ( ) ) {
result = MAV_RESULT_FAILED ;
break ;
}
2015-05-04 23:34:02 -03:00
if ( is_equal ( packet . param1 , 1.0f ) ) {
2015-05-12 04:00:25 -03:00
rover . ins . init_gyro ( ) ;
if ( rover . ins . gyro_calibrated_ok_all ( ) ) {
rover . ahrs . reset_gyro_drift ( ) ;
2015-03-10 20:17:33 -03:00
result = MAV_RESULT_ACCEPTED ;
} else {
result = MAV_RESULT_FAILED ;
}
2015-05-04 23:34:02 -03:00
} else if ( is_equal ( packet . param3 , 1.0f ) ) {
2015-05-12 04:00:25 -03:00
rover . init_barometer ( ) ;
2014-11-20 23:47:09 -04:00
result = MAV_RESULT_ACCEPTED ;
2015-05-04 23:34:02 -03:00
} else if ( is_equal ( packet . param4 , 1.0f ) ) {
2015-05-12 04:00:25 -03:00
rover . trim_radio ( ) ;
2014-11-20 23:47:09 -04:00
result = MAV_RESULT_ACCEPTED ;
2015-05-04 23:34:02 -03:00
} else if ( is_equal ( packet . param5 , 1.0f ) ) {
2015-10-21 18:22:45 -03:00
result = MAV_RESULT_ACCEPTED ;
2015-09-17 09:33:06 -03:00
// start with gyro calibration
rover . ins . init_gyro ( ) ;
// reset ahrs gyro bias
if ( rover . ins . gyro_calibrated_ok_all ( ) ) {
rover . ahrs . reset_gyro_drift ( ) ;
2015-05-15 18:28:42 -03:00
} else {
result = MAV_RESULT_FAILED ;
}
2015-10-21 18:22:45 -03:00
rover . ins . acal_init ( ) ;
rover . ins . get_acal ( ) - > start ( this ) ;
2015-05-15 18:28:42 -03:00
} else if ( is_equal ( packet . param5 , 2.0f ) ) {
2015-09-17 09:33:06 -03:00
// start with gyro calibration
rover . ins . init_gyro ( ) ;
2015-05-15 18:28:42 -03:00
// accel trim
float trim_roll , trim_pitch ;
2015-05-13 00:45:36 -03:00
if ( rover . ins . calibrate_trim ( trim_roll , trim_pitch ) ) {
2015-05-15 18:28:42 -03:00
// reset ahrs's trim to suggested values from calibration routine
2015-05-13 00:45:36 -03:00
rover . ahrs . set_trim ( Vector3f ( trim_roll , trim_pitch , 0 ) ) ;
2015-03-10 20:17:33 -03:00
result = MAV_RESULT_ACCEPTED ;
} else {
result = MAV_RESULT_FAILED ;
}
}
else {
2015-10-25 14:12:30 -03:00
send_text ( MAV_SEVERITY_WARNING , " Unsupported preflight calibration " ) ;
2012-04-30 04:17:14 -03:00
}
break ;
2014-07-09 05:16:47 -03:00
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS :
2015-05-04 23:34:02 -03:00
if ( is_equal ( packet . param1 , 2.0f ) ) {
2014-07-09 05:16:47 -03:00
// save first compass's offsets
2015-05-12 04:00:25 -03:00
rover . compass . set_and_save_offsets ( 0 , packet . param2 , packet . param3 , packet . param4 ) ;
2014-07-09 05:16:47 -03:00
result = MAV_RESULT_ACCEPTED ;
}
2015-05-04 23:34:02 -03:00
if ( is_equal ( packet . param1 , 5.0f ) ) {
2014-07-09 05:16:47 -03:00
// save secondary compass's offsets
2015-05-12 04:00:25 -03:00
rover . compass . set_and_save_offsets ( 1 , packet . param2 , packet . param3 , packet . param4 ) ;
2014-07-09 05:16:47 -03:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
2012-12-18 15:30:42 -04:00
case MAV_CMD_DO_SET_MODE :
switch ( ( uint16_t ) packet . param1 ) {
case MAV_MODE_MANUAL_ARMED :
case MAV_MODE_MANUAL_DISARMED :
2015-05-12 04:00:25 -03:00
rover . set_mode ( MANUAL ) ;
2012-12-18 15:30:42 -04:00
result = MAV_RESULT_ACCEPTED ;
break ;
case MAV_MODE_AUTO_ARMED :
case MAV_MODE_AUTO_DISARMED :
2015-05-12 04:00:25 -03:00
rover . set_mode ( AUTO ) ;
2012-12-18 15:30:42 -04:00
result = MAV_RESULT_ACCEPTED ;
break ;
case MAV_MODE_STABILIZE_DISARMED :
case MAV_MODE_STABILIZE_ARMED :
2015-05-12 04:00:25 -03:00
rover . set_mode ( LEARNING ) ;
2012-12-18 15:30:42 -04:00
result = MAV_RESULT_ACCEPTED ;
break ;
2012-04-30 04:17:14 -03:00
default :
result = MAV_RESULT_UNSUPPORTED ;
2012-12-18 15:30:42 -04:00
}
break ;
case MAV_CMD_DO_SET_SERVO :
2015-05-12 04:00:25 -03:00
if ( rover . ServoRelayEvents . do_set_servo ( packet . param1 , packet . param2 ) ) {
2014-01-20 01:05:23 -04:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
case MAV_CMD_DO_REPEAT_SERVO :
2015-05-12 04:00:25 -03:00
if ( rover . ServoRelayEvents . do_repeat_servo ( packet . param1 , packet . param2 , packet . param3 , packet . param4 * 1000 ) ) {
2014-01-20 01:05:23 -04:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
case MAV_CMD_DO_SET_RELAY :
2015-05-12 04:00:25 -03:00
if ( rover . ServoRelayEvents . do_set_relay ( packet . param1 , packet . param2 ) ) {
2014-01-20 01:05:23 -04:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
case MAV_CMD_DO_REPEAT_RELAY :
2015-05-12 04:00:25 -03:00
if ( rover . ServoRelayEvents . do_repeat_relay ( packet . param1 , packet . param2 , packet . param3 * 1000 ) ) {
2014-01-20 01:05:23 -04:00
result = MAV_RESULT_ACCEPTED ;
}
2012-12-18 15:30:42 -04:00
break ;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN :
2015-05-04 23:34:02 -03:00
if ( is_equal ( packet . param1 , 1.0f ) | | is_equal ( packet . param1 , 3.0f ) ) {
2013-09-03 22:58:41 -03:00
// when packet.param1 == 3 we reboot to hold in bootloader
2015-05-04 23:34:02 -03:00
hal . scheduler - > reboot ( is_equal ( packet . param1 , 3.0f ) ) ;
2012-12-18 15:30:42 -04:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
2015-10-30 02:56:41 -03:00
case MAV_CMD_COMPONENT_ARM_DISARM :
if ( is_equal ( packet . param1 , 1.0f ) ) {
// run pre_arm_checks and arm_checks and display failures
if ( rover . arm_motors ( AP_Arming : : MAVLINK ) ) {
result = MAV_RESULT_ACCEPTED ;
} else {
result = MAV_RESULT_FAILED ;
}
} else if ( is_zero ( packet . param1 ) ) {
if ( rover . disarm_motors ( ) ) {
result = MAV_RESULT_ACCEPTED ;
} else {
result = MAV_RESULT_FAILED ;
}
} else {
result = MAV_RESULT_UNSUPPORTED ;
}
break ;
2015-10-02 08:34:44 -03:00
case MAV_CMD_GET_HOME_POSITION :
2015-12-06 22:30:44 -04:00
if ( rover . home_is_set ! = HOME_UNSET ) {
send_home ( rover . ahrs . get_home ( ) ) ;
result = MAV_RESULT_ACCEPTED ;
}
2015-10-02 08:34:44 -03:00
break ;
2015-02-11 18:04:09 -04:00
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES : {
2015-05-04 23:34:02 -03:00
if ( is_equal ( packet . param1 , 1.0f ) ) {
2015-08-21 14:36:30 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_autopilot_version ( FIRMWARE_VERSION ) ;
2015-02-11 18:04:09 -04:00
result = MAV_RESULT_ACCEPTED ;
}
break ;
}
2015-07-30 22:24:32 -03:00
case MAV_CMD_DO_START_MAG_CAL :
case MAV_CMD_DO_ACCEPT_MAG_CAL :
case MAV_CMD_DO_CANCEL_MAG_CAL :
result = rover . compass . handle_mag_cal_command ( packet ) ;
break ;
2012-12-18 15:30:42 -04:00
default :
2012-04-30 04:17:14 -03:00
break ;
}
2014-03-18 18:54:16 -03:00
mavlink_msg_command_ack_send_buf (
msg ,
2012-04-30 04:17:14 -03:00
chan ,
packet . command ,
result ) ;
break ;
}
case MAVLINK_MSG_ID_SET_MODE :
{
2015-05-24 20:24:11 -03:00
handle_set_mode ( msg , FUNCTOR_BIND ( & rover , & Rover : : mavlink_set_mode , bool , uint8_t ) ) ;
2012-04-30 04:17:14 -03:00
break ;
2014-10-01 01:19:42 -03:00
}
2012-04-30 04:17:14 -03:00
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST :
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
handle_mission_request_list ( rover . mission , msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
// XXX read a WP from EEPROM and send it to the GCS
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_REQUEST :
2014-03-17 23:54:21 -03:00
{
2015-05-12 04:00:25 -03:00
handle_mission_request ( rover . mission , msg ) ;
2014-03-17 23:54:21 -03:00
break ;
}
2012-04-30 04:17:14 -03:00
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_ACK :
2012-04-30 04:17:14 -03:00
{
2014-03-18 18:54:16 -03:00
// not used
2012-04-30 04:17:14 -03:00
break ;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST :
{
2014-08-22 08:11:16 -03:00
// mark the firmware version in the tlog
2015-11-03 01:19:03 -04:00
send_text ( MAV_SEVERITY_INFO , FIRMWARE_STRING ) ;
2014-08-22 08:11:16 -03:00
# if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
2015-11-03 01:19:03 -04:00
send_text ( MAV_SEVERITY_INFO , " PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION ) ;
2014-08-22 08:11:16 -03:00
# endif
2014-03-18 18:54:16 -03:00
handle_param_request_list ( msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2012-12-18 15:30:42 -04:00
case MAVLINK_MSG_ID_PARAM_REQUEST_READ :
{
2014-03-18 18:54:16 -03:00
handle_param_request_read ( msg ) ;
2012-12-18 15:30:42 -04:00
break ;
}
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL :
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
handle_mission_clear_all ( rover . mission , msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_SET_CURRENT :
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
handle_mission_set_current ( rover . mission , msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_COUNT :
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
handle_mission_count ( rover . mission , msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2012-11-27 18:35:09 -04:00
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST :
{
2015-05-12 04:00:25 -03:00
handle_mission_write_partial_list ( rover . mission , msg ) ;
2012-11-27 18:35:09 -04:00
break ;
}
2015-06-23 23:13:24 -03:00
// GCS has sent us a mission item, store to EEPROM
2012-08-08 23:22:46 -03:00
case MAVLINK_MSG_ID_MISSION_ITEM :
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
if ( handle_mission_item ( msg , rover . mission ) ) {
2015-06-23 23:13:24 -03:00
rover . DataFlash . Log_Write_EntireMission ( rover . mission ) ;
2015-05-05 21:17:08 -03:00
}
2012-04-30 04:17:14 -03:00
break ;
}
case MAVLINK_MSG_ID_PARAM_SET :
{
2015-05-12 04:00:25 -03:00
handle_param_set ( msg , & rover . DataFlash ) ;
2012-04-30 04:17:14 -03:00
break ;
2014-03-18 18:54:16 -03:00
}
2012-04-30 04:17:14 -03:00
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE :
2012-12-18 07:44:12 -04:00
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
2015-05-12 04:00:25 -03:00
if ( msg - > sysid ! = rover . g . sysid_my_gcs ) break ; // Only accept control from our gcs
2012-12-18 07:44:12 -04:00
mavlink_rc_channels_override_t packet ;
int16_t v [ 8 ] ;
mavlink_msg_rc_channels_override_decode ( msg , & packet ) ;
v [ 0 ] = packet . chan1_raw ;
v [ 1 ] = packet . chan2_raw ;
v [ 2 ] = packet . chan3_raw ;
v [ 3 ] = packet . chan4_raw ;
v [ 4 ] = packet . chan5_raw ;
v [ 5 ] = packet . chan6_raw ;
v [ 6 ] = packet . chan7_raw ;
v [ 7 ] = packet . chan8_raw ;
hal . rcin - > set_overrides ( v , 8 ) ;
2015-11-19 23:04:16 -04:00
rover . failsafe . rc_override_timer = AP_HAL : : millis ( ) ;
2015-05-12 04:00:25 -03:00
rover . failsafe_trigger ( FAILSAFE_EVENT_RC , false ) ;
2012-12-18 07:44:12 -04:00
break ;
}
2012-04-30 04:17:14 -03:00
case MAVLINK_MSG_ID_HEARTBEAT :
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
2015-05-12 04:00:25 -03:00
if ( msg - > sysid ! = rover . g . sysid_my_gcs ) break ;
2015-11-19 23:04:16 -04:00
rover . last_heartbeat_ms = rover . failsafe . rc_override_timer = AP_HAL : : millis ( ) ;
2015-05-12 04:00:25 -03:00
rover . failsafe_trigger ( FAILSAFE_EVENT_GCS , false ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2012-12-18 16:17:06 -04:00
# if HIL_MODE != HIL_MODE_DISABLED
2012-04-30 04:17:14 -03:00
case MAVLINK_MSG_ID_HIL_STATE :
{
mavlink_hil_state_t packet ;
mavlink_msg_hil_state_decode ( msg , & packet ) ;
// set gps hil sensor
2014-03-30 22:01:54 -03:00
Location loc ;
loc . lat = packet . lat ;
loc . lng = packet . lon ;
loc . alt = packet . alt / 10 ;
Vector3f vel ( packet . vx , packet . vy , packet . vz ) ;
vel * = 0.01f ;
2014-04-01 17:51:15 -03:00
gps . setHIL ( 0 , AP_GPS : : GPS_OK_FIX_3D ,
2014-03-30 22:01:54 -03:00
packet . time_usec / 1000 ,
2014-04-01 17:51:15 -03:00
loc , vel , 10 , 0 , true ) ;
2012-04-30 04:17:14 -03:00
// rad/sec
Vector3f gyros ;
2012-12-18 16:17:06 -04:00
gyros . x = packet . rollspeed ;
gyros . y = packet . pitchspeed ;
gyros . z = packet . yawspeed ;
2012-12-18 15:30:42 -04:00
2012-04-30 04:17:14 -03:00
// m/s/s
Vector3f accels ;
2013-01-10 14:42:24 -04:00
accels . x = packet . xacc * ( GRAVITY_MSS / 1000.0f ) ;
accels . y = packet . yacc * ( GRAVITY_MSS / 1000.0f ) ;
accels . z = packet . zacc * ( GRAVITY_MSS / 1000.0f ) ;
2012-12-18 16:17:06 -04:00
2014-02-22 17:18:15 -04:00
ins . set_gyro ( 0 , gyros ) ;
2012-04-30 04:17:14 -03:00
2014-02-22 17:18:15 -04:00
ins . set_accel ( 0 , accels ) ;
2015-05-15 01:04:51 -03:00
compass . setHIL ( 0 , packet . roll , packet . pitch , packet . yaw ) ;
compass . setHIL ( 1 , packet . roll , packet . pitch , packet . yaw ) ;
2013-06-03 22:57:59 -03:00
break ;
2012-04-30 04:17:14 -03:00
}
# endif // HIL_MODE
2013-07-14 20:57:00 -03:00
# if CAMERA == ENABLED
2015-09-06 17:44:51 -03:00
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
2013-07-14 20:57:00 -03:00
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE :
{
break ;
}
2015-09-06 17:44:51 -03:00
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
2013-07-14 20:57:00 -03:00
case MAVLINK_MSG_ID_DIGICAM_CONTROL :
{
2015-05-12 04:00:25 -03:00
rover . camera . control_msg ( msg ) ;
rover . log_picture ( ) ;
2013-07-14 20:57:00 -03:00
break ;
}
# endif // CAMERA == ENABLED
2012-04-30 04:17:14 -03:00
# if MOUNT == ENABLED
2015-09-06 17:44:51 -03:00
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
2012-04-30 04:17:14 -03:00
case MAVLINK_MSG_ID_MOUNT_CONFIGURE :
{
2015-05-12 04:00:25 -03:00
rover . camera_mount . configure_msg ( msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
2015-09-06 17:44:51 -03:00
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
2012-04-30 04:17:14 -03:00
case MAVLINK_MSG_ID_MOUNT_CONTROL :
{
2015-05-12 04:00:25 -03:00
rover . camera_mount . control_msg ( msg ) ;
2012-04-30 04:17:14 -03:00
break ;
}
# endif // MOUNT == ENABLED
2012-05-14 15:33:03 -03:00
case MAVLINK_MSG_ID_RADIO :
2013-08-24 04:58:37 -03:00
case MAVLINK_MSG_ID_RADIO_STATUS :
2012-05-14 15:33:03 -03:00
{
2015-05-12 04:00:25 -03:00
handle_radio_status ( msg , rover . DataFlash , rover . should_log ( MASK_LOG_PM ) ) ;
2012-05-14 15:33:03 -03:00
break ;
}
2014-01-14 00:10:13 -04:00
case MAVLINK_MSG_ID_LOG_REQUEST_DATA :
case MAVLINK_MSG_ID_LOG_ERASE :
2015-05-12 04:00:25 -03:00
rover . in_log_download = true ;
2015-10-03 00:31:34 -03:00
/* no break */
2014-01-14 00:10:13 -04:00
case MAVLINK_MSG_ID_LOG_REQUEST_LIST :
2015-05-12 04:00:25 -03:00
if ( ! rover . in_mavlink_delay ) {
handle_log_message ( msg , rover . DataFlash ) ;
2014-01-14 00:10:13 -04:00
}
break ;
case MAVLINK_MSG_ID_LOG_REQUEST_END :
2015-05-12 04:00:25 -03:00
rover . in_log_download = false ;
if ( ! rover . in_mavlink_delay ) {
handle_log_message ( msg , rover . DataFlash ) ;
2013-12-15 19:35:27 -04:00
}
break ;
2014-04-03 23:55:32 -03:00
case MAVLINK_MSG_ID_SERIAL_CONTROL :
2015-05-12 04:00:25 -03:00
handle_serial_control ( msg , rover . gps ) ;
2014-04-03 23:55:32 -03:00
break ;
2015-03-25 01:01:20 -03:00
case MAVLINK_MSG_ID_GPS_INJECT_DATA :
2015-05-12 04:00:25 -03:00
handle_gps_inject ( msg , rover . gps ) ;
2015-03-25 01:01:20 -03:00
break ;
2014-04-03 23:55:32 -03:00
2015-11-10 02:26:09 -04:00
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS :
rover . DataFlash . remote_log_block_status_msg ( chan , msg ) ;
break ;
2015-02-11 04:52:38 -04:00
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST :
2015-08-21 14:36:30 -03:00
rover . gcs [ chan - MAVLINK_COMM_0 ] . send_autopilot_version ( FIRMWARE_VERSION ) ;
2015-02-11 04:52:38 -04:00
break ;
2012-04-30 04:17:14 -03:00
} // end switch
} // end handle mavlink
/*
2012-12-18 07:44:12 -04:00
* a delay ( ) callback that processes MAVLink packets . We set this as the
* callback in long running library initialisation routines to allow
* MAVLink to process packets while waiting for the initialisation to
* complete
*/
2015-05-12 02:03:23 -03:00
void Rover : : mavlink_delay_cb ( )
2012-04-30 04:17:14 -03:00
{
2015-05-12 04:00:25 -03:00
static uint32_t last_1hz , last_50hz , last_5s ;
2014-01-13 23:40:10 -04:00
if ( ! gcs [ 0 ] . initialised | | in_mavlink_delay ) return ;
2012-04-30 04:17:14 -03:00
in_mavlink_delay = true ;
2012-12-18 07:44:12 -04:00
uint32_t tnow = millis ( ) ;
if ( tnow - last_1hz > 1000 ) {
last_1hz = tnow ;
gcs_send_message ( MSG_HEARTBEAT ) ;
gcs_send_message ( MSG_EXTENDED_STATUS1 ) ;
}
if ( tnow - last_50hz > 20 ) {
last_50hz = tnow ;
gcs_update ( ) ;
gcs_data_stream_send ( ) ;
2013-08-29 00:14:16 -03:00
notify . update ( ) ;
2012-12-18 07:44:12 -04:00
}
if ( tnow - last_5s > 5000 ) {
last_5s = tnow ;
2015-11-18 14:53:14 -04:00
gcs_send_text ( MAV_SEVERITY_INFO , " Initialising APM " ) ;
2012-12-18 07:44:12 -04:00
}
check_usb_mux ( ) ;
2012-04-30 04:17:14 -03:00
in_mavlink_delay = false ;
}
/*
2012-12-18 15:30:42 -04:00
* send a message on both GCS links
2012-04-30 04:17:14 -03:00
*/
2015-05-12 02:03:23 -03:00
void Rover : : gcs_send_message ( enum ap_message id )
2012-04-30 04:17:14 -03:00
{
2013-11-23 06:57:26 -04:00
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
gcs [ i ] . send_message ( id ) ;
}
2012-04-30 04:17:14 -03:00
}
}
2015-07-24 04:31:30 -03:00
/*
* send a mission item reached message and load the index before the send attempt in case it may get delayed
*/
void Rover : : gcs_send_mission_item_reached_message ( uint16_t mission_index )
{
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
gcs [ i ] . mission_item_reached_index = mission_index ;
gcs [ i ] . send_message ( MSG_MISSION_ITEM_REACHED ) ;
}
}
}
2012-04-30 04:17:14 -03:00
/*
2012-12-18 15:30:42 -04:00
* send data streams in the given rate range on both links
2012-04-30 04:17:14 -03:00
*/
2015-05-12 02:03:23 -03:00
void Rover : : gcs_data_stream_send ( void )
2012-04-30 04:17:14 -03:00
{
2013-11-23 06:57:26 -04:00
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
gcs [ i ] . data_stream_send ( ) ;
}
2012-04-30 04:17:14 -03:00
}
}
/*
2012-12-18 15:30:42 -04:00
* look for incoming commands on the GCS links
2012-04-30 04:17:14 -03:00
*/
2015-05-12 02:03:23 -03:00
void Rover : : gcs_update ( void )
2012-04-30 04:17:14 -03:00
{
2013-11-23 06:57:26 -04:00
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
2014-05-20 23:43:26 -03:00
# if CLI_ENABLED == ENABLED
2015-05-24 20:24:11 -03:00
gcs [ i ] . update ( g . cli_enabled = = 1 ? FUNCTOR_BIND_MEMBER ( & Rover : : run_cli , void , AP_HAL : : UARTDriver * ) : NULL ) ;
2014-05-20 23:43:26 -03:00
# else
gcs [ i ] . update ( NULL ) ;
# endif
2013-11-23 06:57:26 -04:00
}
2012-04-30 04:17:14 -03:00
}
}
2015-10-26 08:25:44 -03:00
void Rover : : gcs_send_text ( MAV_SEVERITY severity , const char * str )
2012-04-30 04:17:14 -03:00
{
2013-11-23 06:57:26 -04:00
for ( uint8_t i = 0 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
2015-10-25 14:12:30 -03:00
gcs [ i ] . send_text ( severity , str ) ;
2013-11-23 06:57:26 -04:00
}
2012-04-30 04:17:14 -03:00
}
2013-11-23 06:57:26 -04:00
# if LOGGING_ENABLED == ENABLED
2015-10-26 08:23:55 -03:00
DataFlash . Log_Write_Message ( str ) ;
2013-11-23 06:57:26 -04:00
# endif
2012-04-30 04:17:14 -03:00
}
/*
2012-12-18 07:44:12 -04:00
* send a low priority formatted message to the GCS
* only one fits in the queue , so if you send more than one before the
* last one gets into the serial buffer then the old one will be lost
2012-04-30 04:17:14 -03:00
*/
2015-11-03 01:19:03 -04:00
void Rover : : gcs_send_text_fmt ( MAV_SEVERITY severity , const char * fmt , . . . )
2012-04-30 04:17:14 -03:00
{
2012-12-18 07:44:12 -04:00
va_list arg_list ;
2015-11-03 01:19:03 -04:00
gcs [ 0 ] . pending_status . severity = ( uint8_t ) severity ;
2012-12-18 07:44:12 -04:00
va_start ( arg_list , fmt ) ;
2015-10-24 19:07:07 -03:00
hal . util - > vsnprintf ( ( char * ) gcs [ 0 ] . pending_status . text ,
2013-11-23 06:57:26 -04:00
sizeof ( gcs [ 0 ] . pending_status . text ) , fmt , arg_list ) ;
2012-12-18 07:44:12 -04:00
va_end ( arg_list ) ;
2013-11-23 06:57:26 -04:00
# if LOGGING_ENABLED == ENABLED
DataFlash . Log_Write_Message ( gcs [ 0 ] . pending_status . text ) ;
# endif
2014-03-18 21:07:15 -03:00
gcs [ 0 ] . send_message ( MSG_STATUSTEXT ) ;
2013-11-23 06:57:26 -04:00
for ( uint8_t i = 1 ; i < num_gcs ; i + + ) {
if ( gcs [ i ] . initialised ) {
gcs [ i ] . pending_status = gcs [ 0 ] . pending_status ;
2014-03-18 21:07:15 -03:00
gcs [ i ] . send_message ( MSG_STATUSTEXT ) ;
2013-11-23 06:57:26 -04:00
}
2012-04-30 04:17:14 -03:00
}
}
2012-05-14 15:33:03 -03:00
2013-10-27 20:33:52 -03:00
/**
retry any deferred messages
*/
2015-05-12 02:03:23 -03:00
void Rover : : gcs_retry_deferred ( void )
2013-10-27 20:33:52 -03:00
{
gcs_send_message ( MSG_RETRY_DEFERRED ) ;
}