Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: create a Mode hierarchy to handle different control modes

This commit is contained in:
Peter Barker 2019-09-14 01:41:55 +10:00 committed by Peter Barker
parent dafd030904
commit 531bdcc130
15 changed files with 236 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
AntennaTracker/GCS_Mavlink.cpp
View File

@ -30,17 +30,17 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
// 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 (tracker.control_mode) {
case MANUAL:
switch (tracker.mode->number()) {
case Mode::Number::MANUAL:
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case STOP:
case Mode::Number::STOP:
break;
case SCAN:
case SERVO_TEST:
case AUTO:
case Mode::Number::SCAN:
case Mode::Number::SERVOTEST:
case Mode::Number::AUTO:
_base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
@ -48,13 +48,13 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
// positions", which APM does not currently do
break;
case INITIALISING:
case Mode::Number::INITIALISING:
break;
}
// we are armed if safety switch is not disarmed
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED &&
tracker.control_mode != INITIALISING &&
tracker.mode != &tracker.mode_initialising &&
hal.util->get_soft_armed()) {
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@ -64,12 +64,12 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
uint32_t GCS_Tracker::custom_mode() const
{
return tracker.control_mode;
return (uint32_t)tracker.mode->number();
}
MAV_STATE GCS_MAVLINK_Tracker::system_status() const
{
if (tracker.control_mode == INITIALISING) {
if (tracker.mode == &tracker.mode_initialising) {
return MAV_STATE_CALIBRATING;
}
return MAV_STATE_ACTIVE;
@ -399,14 +399,17 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_DO_SET_SERVO:
if (!tracker.servo_test_set_servo(packet.param1, packet.param2)) {
// ensure we are in servo test mode
tracker.set_mode(tracker.mode_servotest, ModeReason::SERVOTEST);
if (!tracker.mode_servotest.set_servo(packet.param1, packet.param2)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
tracker.set_mode(AUTO, ModeReason::GCS_COMMAND);
tracker.set_mode(tracker.mode_auto, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
default:

2
AntennaTracker/Log.cpp
View File

@ -78,7 +78,7 @@ const struct LogStructure Tracker::log_structure[] = {
void Tracker::Log_Write_Vehicle_Startup_Messages()
{
logger.Write_Mode(control_mode, ModeReason::INITIALISED);
logger.Write_Mode((uint8_t)mode->number(), ModeReason::INITIALISED);
gps.Write_AP_Logger_Log_Startup_messages();
}

26
AntennaTracker/Tracker.cpp
View File

@ -130,6 +130,32 @@ void Tracker::ten_hz_logging_loop()
}
}
Mode *Tracker::mode_from_mode_num(const Mode::Number num)
{
Mode *ret = nullptr;
switch (num) {
case Mode::Number::MANUAL:
ret = &mode_manual;
break;
case Mode::Number::STOP:
ret = &mode_stop;
break;
case Mode::Number::SCAN:
ret = &mode_scan;
break;
case Mode::Number::SERVOTEST:
ret = &mode_servotest;
break;
case Mode::Number::AUTO:
ret = &mode_auto;
break;
case Mode::Number::INITIALISING:
ret = &mode_initialising;
break;
}
return ret;
}
/*
update AP_Stats
*/

39
AntennaTracker/Tracker.h
View File

@ -75,11 +75,15 @@
#include <SITL/SITL.h>
#endif
#include "mode.h"
class Tracker : public AP_Vehicle {
public:
friend class GCS_MAVLINK_Tracker;
friend class GCS_Tracker;
friend class Parameters;
friend class ModeAuto;
friend class Mode;
Tracker(void);
@ -135,7 +139,16 @@ private:
nullptr};
struct Location current_loc;
enum ControlMode control_mode = INITIALISING;
Mode *mode_from_mode_num(enum Mode::Number num);
Mode *mode = &mode_initialising;
ModeAuto mode_auto;
ModeInitialising mode_initialising;
ModeManual mode_manual;
ModeScan mode_scan;
ModeServoTest mode_servotest;
ModeStop mode_stop;
#ifdef ENABLE_SCRIPTING
AP_Scripting scripting;
@ -153,7 +166,7 @@ private:
} vehicle;
// Navigation controller state
struct {
struct NavStatus {
float bearing; // bearing to vehicle in centi-degrees
float distance; // distance to vehicle in meters
float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
@ -167,7 +180,7 @@ private:
bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup)
bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode
bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode
} nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
} nav_status;
// setup the var_info table
AP_Param param_loader{var_info};
@ -187,22 +200,6 @@ private:
void ten_hz_logging_loop();
void stats_update();
// control_auto.cpp
void update_auto(void);
void calc_angle_error(float pitch, float yaw, bool direction_reversed);
void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
bool get_ef_yaw_direction();
// control_manual.cpp
void update_manual(void);
// control_scan.cpp
void update_scan(void);
// control_servo_test.cpp
bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm);
// GCS_Mavlink.cpp
void send_nav_controller_output(mavlink_channel_t chan);
@ -246,8 +243,8 @@ private:
void arm_servos();
void disarm_servos();
void prepare_servos();
void set_mode(enum ControlMode mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
void set_mode(Mode &newmode, ModeReason reason);
bool set_mode(uint8_t new_mode, ModeReason reason) override;
bool should_log(uint32_t mask);
bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
void exit_mission_callback() { return; }

38
AntennaTracker/control_auto.cpp
View File

@ -6,10 +6,14 @@
/*
* update_auto - runs the auto controller
* called at 50hz while control_mode is 'AUTO'
* called at 50hz while control mode is 'AUTO'
*/
void Tracker::update_auto(void)
void Mode::update_auto(void)
{
struct Tracker::NavStatus &nav_status = tracker.nav_status;
Parameters &g = tracker.g;
float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
@ -22,17 +26,18 @@ void Tracker::update_auto(void)
convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
// only move servos if target is at least distance_min away if we have a target
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !vehicle.location_valid) {
update_pitch_servo(bf_pitch);
update_yaw_servo(bf_yaw);
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
tracker.update_pitch_servo(bf_pitch);
tracker.update_yaw_servo(bf_yaw);
}
}
void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
// Pitch angle error in centidegrees
// Positive error means the target is above current pitch
// Negative error means the target is below current pitch
const AP_AHRS &ahrs = AP::ahrs();
float ahrs_pitch = ahrs.pitch_sensor;
int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
@ -53,25 +58,29 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
float bf_pitch_err;
float bf_yaw_err;
convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
struct Tracker::NavStatus &nav_status = tracker.nav_status;
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
// set actual and desired for logging, note we are using angles not rates
Parameters &g = tracker.g;
g.pidPitch2Srv.set_target_rate(pitch * 0.01);
g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
g.pidYaw2Srv.set_target_rate(yaw * 0.01);
g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
}
void Tracker::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
{
// earth frame to body frame pitch and yaw conversion
const AP_AHRS &ahrs = AP::ahrs();
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
}
bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
{
const AP_AHRS &ahrs = AP::ahrs();
// avoid divide by zero
if (is_zero(ahrs.cos_pitch())) {
return false;
@ -83,13 +92,14 @@ bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& e
}
// return value is true if taking the long road to the target, false if normal, shortest direction should be used
bool Tracker::get_ef_yaw_direction()
bool Mode::get_ef_yaw_direction()
{
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();
Parameters &g = tracker.g;
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();
// distances to earthframe angle limits in centi-degrees
float ef_yaw_limit_lower = yaw_angle_limit_lower;
@ -99,7 +109,9 @@ bool Tracker::get_ef_yaw_direction()
convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
const AP_AHRS &ahrs = AP::ahrs();
float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
struct Tracker::NavStatus &nav_status = tracker.nav_status;
float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);

4
AntennaTracker/control_manual.cpp
View File

@ -6,9 +6,9 @@
/*
* update_manual - runs the manual controller
* called at 50hz while control_mode is 'MANUAL'
* called at 50hz while control mode is 'MANUAL'
*/
void Tracker::update_manual(void)
void ModeManual::update()
{
// copy yaw and pitch input to output
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, RC_Channels::rc_channel(CH_YAW)->get_radio_in());

8
AntennaTracker/control_scan.cpp
View File

@ -6,10 +6,14 @@
/*
* update_scan - runs the scan controller
* called at 50hz while control_mode is 'SCAN'
* called at 50hz while control mode is 'SCAN'
*/
void Tracker::update_scan(void)
void Mode::update_scan(void)
{
struct Tracker::NavStatus &nav_status = tracker.nav_status;
Parameters &g = tracker.g;
if (!nav_status.manual_control_yaw) {
float yaw_delta = g.scan_speed_yaw * 0.02f;
nav_status.bearing += yaw_delta * (nav_status.scan_reverse_yaw?-1:1);

9
AntennaTracker/control_servo_test.cpp
View File

@ -5,10 +5,10 @@
*/
/*
* servo_test_set_servo - sets the yaw or pitch servo pwm directly
* set_servo - sets the yaw or pitch servo pwm directly
* servo_num are 1 for yaw, 2 for pitch
*/
bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
bool ModeServoTest::set_servo(uint8_t servo_num, uint16_t pwm)
{
// convert servo_num from 1~2 to 0~1 range
servo_num--;
@ -18,11 +18,6 @@ bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
return false;
}
// ensure we are in servo test mode
if (control_mode != SERVO_TEST) {
set_mode(SERVO_TEST, ModeReason::SERVOTEST);
}
// set yaw servo pwm and send output to servo
if (servo_num == CH_YAW) {
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, pwm);

9
AntennaTracker/defines.h
View File

@ -3,15 +3,6 @@
// Controller modes
// ----------------
enum ControlMode {
MANUAL=0,
STOP=1,
SCAN=2,
SERVO_TEST=3,
AUTO=10,
INITIALISING=16
};
enum ServoType {
SERVO_TYPE_POSITION=0,
SERVO_TYPE_ONOFF=1,

3
AntennaTracker/mode.cpp Normal file
View File

@ -0,0 +1,3 @@
#include "mode.h"
#include "Tracker.h"

82
AntennaTracker/mode.h Normal file
View File

@ -0,0 +1,82 @@
#pragma once
#include <stdint.h>
class Mode {
public:
enum class Number {
MANUAL=0,
STOP=1,
SCAN=2,
SERVOTEST=3,
AUTO=10,
INITIALISING=16
};
Mode() {}
// do not allow copying
Mode(const Mode &other) = delete;
Mode &operator=(const Mode&) = delete;
// returns a unique number specific to this mode
virtual Mode::Number number() const = 0;
virtual bool requires_armed_servos() const = 0;
virtual void update() = 0;
protected:
void update_scan();
void update_auto();
bool get_ef_yaw_direction();
void calc_angle_error(float pitch, float yaw, bool direction_reversed);
void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
};
class ModeAuto : public Mode {
public:
Mode::Number number() const override { return Mode::Number::AUTO; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
class ModeInitialising : public Mode {
public:
Mode::Number number() const override { return Mode::Number::INITIALISING; }
bool requires_armed_servos() const override { return false; }
void update() override {};
};
class ModeManual : public Mode {
public:
Mode::Number number() const override { return Mode::Number::MANUAL; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
class ModeScan : public Mode {
public:
Mode::Number number() const override { return Mode::Number::SCAN; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
class ModeServoTest : public Mode {
public:
Mode::Number number() const override { return Mode::Number::SERVOTEST; }
bool requires_armed_servos() const override { return true; }
void update() override {};
bool set_servo(uint8_t servo_num, uint16_t pwm);
};
class ModeStop : public Mode {
public:
Mode::Number number() const override { return Mode::Number::STOP; }
bool requires_armed_servos() const override { return false; }
void update() override {};
};

12
AntennaTracker/mode_auto.cpp Normal file
View File

@ -0,0 +1,12 @@
#include "mode.h"
#include "Tracker.h"
void ModeAuto::update()
{
if (tracker.vehicle.location_valid) {
update_auto();
} else if (tracker.target_set) {
update_scan();
}
}

6
AntennaTracker/mode_scan.cpp Normal file
View File

@ -0,0 +1,6 @@
#include "mode.h"
void ModeScan::update()
{
update_scan();
}

76
AntennaTracker/system.cpp
View File

@ -108,24 +108,11 @@ void Tracker::init_tracker()
gcs().send_text(MAV_SEVERITY_INFO,"Ready to track");
hal.scheduler->delay(1000); // Why????
switch (g.initial_mode) {
case MANUAL:
set_mode(MANUAL, ModeReason::STARTUP);
break;
case SCAN:
set_mode(SCAN, ModeReason::STARTUP);
break;
case STOP:
set_mode(STOP, ModeReason::STARTUP);
break;
case AUTO:
default:
set_mode(AUTO, ModeReason::STARTUP);
break;
Mode *newmode = mode_from_mode_num((Mode::Number)g.initial_mode.get());
if (newmode == nullptr) {
newmode = &mode_manual;
}
set_mode(*newmode, ModeReason::STARTUP);
if (g.startup_delay > 0) {
// arm servos with trim value to allow them to start up (required
@ -214,47 +201,54 @@ void Tracker::prepare_servos()
SRV_Channels::output_ch_all();
}
void Tracker::set_mode(enum ControlMode mode, ModeReason reason)
void Tracker::set_mode(Mode &newmode, const ModeReason reason)
{
if (control_mode == mode) {
if (mode == &newmode) {
// don't switch modes if we are already in the correct mode.
return;
}
control_mode = mode;
mode = &newmode;
switch (control_mode) {
case AUTO:
case MANUAL:
case SCAN:
case SERVO_TEST:
if (mode->requires_armed_servos()) {
arm_servos();
break;
case STOP:
case INITIALISING:
} else {
disarm_servos();
break;
}
// log mode change
logger.Write_Mode(control_mode, reason);
gcs().send_message(MSG_HEARTBEAT);
logger.Write_Mode((uint8_t)mode->number(), reason);
gcs().send_message(MSG_HEARTBEAT);
nav_status.bearing = ahrs.yaw_sensor * 0.01f;
}
bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
{
switch (new_mode) {
case AUTO:
case MANUAL:
case SCAN:
case SERVO_TEST:
case STOP:
set_mode((enum ControlMode)new_mode, reason);
return true;
Mode *fred = nullptr;
switch ((Mode::Number)new_mode) {
case Mode::Number::INITIALISING:
return false;
case Mode::Number::AUTO:
fred = &mode_auto;
break;
case Mode::Number::MANUAL:
fred = &mode_manual;
break;
case Mode::Number::SCAN:
fred = &mode_scan;
break;
case Mode::Number::SERVOTEST:
fred = &mode_servotest;
break;
case Mode::Number::STOP:
fred = &mode_stop;
break;
}
return false;
if (fred == nullptr) {
return false;
}
set_mode(*fred, reason);
return true;
}
/*

27
AntennaTracker/tracking.cpp
View File

@ -53,7 +53,7 @@ void Tracker::update_bearing_and_distance()
// calculate bearing to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_yaw) {
if (mode != &mode_scan && !nav_status.manual_control_yaw) {
nav_status.bearing = current_loc.get_bearing_to(vehicle.location_estimate) * 0.01f;
}
@ -70,7 +70,7 @@ void Tracker::update_bearing_and_distance()
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
if (mode->number() != Mode::Number::SCAN && !nav_status.manual_control_pitch) {
if (g.alt_source == ALT_SOURCE_BARO) {
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
} else {
@ -117,28 +117,7 @@ void Tracker::update_tracking(void)
break;
}
} else {
switch (control_mode) {
case AUTO:
if (vehicle.location_valid) {
update_auto();
} else if (tracker.target_set) {
update_scan();
}
break;
case MANUAL:
update_manual();
break;
case SCAN:
update_scan();
break;
case SERVO_TEST:
case STOP:
case INITIALISING:
break;
}
mode->update();
}
// convert servo_out to radio_out and send to servo