CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Rover/GCS_Rover.cpp
Views: 1798
1
#include "GCS_Rover.h"

2


3
#include "Rover.h"

4


5
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>

6


7
uint8_t GCS_Rover::sysid_this_mav() const

8
{

9
    return rover.g.sysid_this_mav;

10
}

11


12
bool GCS_Rover::simple_input_active() const

13
{

14
    if (rover.control_mode != &rover.mode_simple) {

15
        return false;

16
    }

17
    return (rover.g2.simple_type == ModeSimple::Simple_InitialHeading);

18
}

19


20
bool GCS_Rover::supersimple_input_active() const

21
{

22
    if (rover.control_mode != &rover.mode_simple) {

23
        return false;

24
    }

25
    return (rover.g2.simple_type == ModeSimple::Simple_CardinalDirections);

26
}

27


28
void GCS_Rover::update_vehicle_sensor_status_flags(void)

29
{

30
    // mode-specific:

31
    control_sensors_present |=

32
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

33
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |

34
        MAV_SYS_STATUS_SENSOR_YAW_POSITION |

35
        MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

36


37
    if (rover.control_mode->attitude_stabilized()) {

38
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control

39
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control

40
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control

41
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control

42
    }

43
    if (rover.control_mode->is_autopilot_mode()) {

44
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position

45
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position

46
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control

47
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control

48
    }

49


50
#if HAL_PROXIMITY_ENABLED

51
    const AP_Proximity *proximity = AP_Proximity::get_singleton();

52
    if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) {

53
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

54
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

55
    }

56
    if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) {

57
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

58
    }

59
#endif

60


61
#if AP_RANGEFINDER_ENABLED

62
    const RangeFinder *rangefinder = RangeFinder::get_singleton();

63
    if (rangefinder && rangefinder->num_sensors() > 0) {

64
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

65
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

66
        AP_RangeFinder_Backend *s = rangefinder->get_backend(0);

67
        if (s != nullptr && s->has_data()) {

68
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

69
        }

70
    }

71
#endif

72
}

73


74