mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-19 15:18:28 -04:00
150 lines
3.8 KiB
C++
150 lines
3.8 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
Simulator for Volz servos
|
|
|
|
./Tools/autotest/sim_vehicle.py --gdb --debug -v Plane -A --serial5=sim:volz --speedup=1 --console -f plane-redundant
|
|
|
|
param set SERIAL5_PROTOCOL 14 // Volz
|
|
param set SERIAL5_BAUD 115200
|
|
param set SERVO_VOLZ_MASK 1803
|
|
param set SIM_VOLZ_ENA 1
|
|
param set SIM_VOLZ_MASK 1803
|
|
reboot
|
|
|
|
param fetch
|
|
|
|
#param set SIM_VOLZ_FMASK 1 # fail mask
|
|
|
|
./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter.Volz
|
|
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "SIM_config.h"
|
|
|
|
#if AP_SIM_VOLZ_ENABLED
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include "SITL_Input.h"
|
|
|
|
#include "SIM_SerialDevice.h"
|
|
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
|
|
namespace SITL {
|
|
|
|
class Volz : public SerialDevice {
|
|
public:
|
|
|
|
Volz();
|
|
|
|
// update state
|
|
void update(const class Aircraft &aircraft);
|
|
|
|
static const AP_Param::GroupInfo var_info[];
|
|
|
|
bool enabled() const { return _enabled.get(); }
|
|
|
|
void update_sitl_input_pwm(struct sitl_input &input);
|
|
|
|
private:
|
|
|
|
// parameters / configuration
|
|
AP_Int8 _enabled; // enable FETtec servo sim
|
|
AP_Int32 _output_mask; // mask of servo outputs to override
|
|
AP_Int32 _failed_mask; // mask of servo outputs to mark as failed
|
|
|
|
// packet abstraction; "struct Command" is used for both reading
|
|
// and writing to the autopilot.
|
|
enum class CommandId : uint8_t {
|
|
SET_EXTENDED_POSITION = 0xDC,
|
|
EXTENDED_POSITION_RESPONSE = 0x2C,
|
|
READ_CURRENT = 0xB0,
|
|
CURRENT_RESPONSE = 0x30,
|
|
READ_VOLTAGE = 0xB1,
|
|
VOLTAGE_RESPONSE = 0x31,
|
|
READ_TEMPERATURE = 0xC0,
|
|
TEMPERATURE_RESPONSE = 0x10,
|
|
};
|
|
struct PACKED Command {
|
|
CommandId command_id;
|
|
uint8_t actuator_id; // actuator send to or receiving from
|
|
uint8_t arg1; // CMD dependant argument 1
|
|
uint8_t arg2; // CMD dependant argument 2
|
|
uint16_t crc;
|
|
|
|
uint16_t calculate_checksum() const {
|
|
return crc_crc16_ibm(0xffff, (uint8_t*)this, 4);
|
|
}
|
|
void update_checksum() {
|
|
crc = htobe16(calculate_checksum());
|
|
}
|
|
};
|
|
|
|
// reading-from-autopilot support:
|
|
union {
|
|
Command command;
|
|
uint8_t buffer[128];
|
|
} u;
|
|
uint8_t buflen;
|
|
bool shift_command_to_front_of_buffer();
|
|
void consume_command();
|
|
void process_command(const Command &command);
|
|
void update_servos(const class Aircraft &aircraft);
|
|
void update_input();
|
|
|
|
// class creating a servo abstraction:
|
|
class Servo {
|
|
public:
|
|
uint8_t id;
|
|
|
|
float position; // -1 to 1
|
|
float desired_position;
|
|
float pcb_temperature;
|
|
float motor_temperature;
|
|
float primary_current;
|
|
float secondary_current;
|
|
float primary_voltage;
|
|
float secondary_voltage;
|
|
|
|
uint16_t pwm() {
|
|
if (failed) {
|
|
return last_good_pwm;
|
|
}
|
|
last_good_pwm = 1500 + desired_position*500;
|
|
return last_good_pwm;
|
|
}
|
|
|
|
bool failed;
|
|
uint16_t last_good_pwm;
|
|
};
|
|
|
|
// the simulated servos:
|
|
Servo servos[16];
|
|
|
|
// last time we updated the servo physics:
|
|
uint32_t last_servo_update_us;
|
|
|
|
|
|
void log_Servo(const Volz::Servo &servo, const uint32_t now_us);
|
|
};
|
|
|
|
}
|
|
|
|
#endif // AP_SIM_VOLZ_ENABLED
|