mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-31 21:18:29 -04:00
266 lines
8.2 KiB
C++
266 lines
8.2 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 the Volz
|
|
|
|
TODO:
|
|
- check that -128-to-127 thing vs -100-to-100
|
|
|
|
*/
|
|
|
|
#include "SIM_config.h"
|
|
|
|
#if AP_SIM_VOLZ_ENABLED
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include "SIM_Volz.h"
|
|
#include "SITL.h"
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#include "SIM_Aircraft.h"
|
|
|
|
#include <errno.h>
|
|
|
|
using namespace SITL;
|
|
|
|
// table of user settable parameters
|
|
const AP_Param::GroupInfo Volz::var_info[] = {
|
|
|
|
// @Param: ENA
|
|
// @DisplayName: Volz simulator enable/disable
|
|
// @Description: Allows you to enable (1) or disable (0) the Volz simulator
|
|
// @Values: 0:Disabled,1:Enabled
|
|
// @User: Advanced
|
|
AP_GROUPINFO("ENA", 1, Volz, _enabled, 0),
|
|
|
|
// @Param: MASK
|
|
// @DisplayName: Volz override mask
|
|
// @Description: mask of servo output channels to override with values from Volz protocol. Note these are indexed from 0 - so channel 3 (usually throttle) has value 4 in this bitmask (1<<2).
|
|
// @User: Advanced
|
|
AP_GROUPINFO("MASK", 2, Volz, _output_mask, (1U<<0)|(1U<<1)|(1U<<3)),
|
|
|
|
// @Param: FMASK
|
|
// @DisplayName: Volz fail mask
|
|
// @Description: fail servo at current position. Channel 1 is bit 0.
|
|
// @User: Advanced
|
|
AP_GROUPINFO("FMASK", 3, Volz, _failed_mask, 0),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
Volz::Volz() : SerialDevice::SerialDevice()
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
// initialise IDs
|
|
for (uint8_t n=0; n<ARRAY_SIZE(servos); n++) {
|
|
auto &s = servos[n];
|
|
s.id = n+1; // really should parameterise this
|
|
s.pcb_temperature = 25;
|
|
s.motor_temperature = 25;
|
|
s.primary_current = 0.1;
|
|
s.secondary_current = 0.1;
|
|
s.primary_voltage = 1.1;
|
|
s.secondary_voltage = 1.2;
|
|
}
|
|
}
|
|
|
|
void Volz::update_servos(const class Aircraft &aircraft)
|
|
{
|
|
const float ambient_degC = aircraft.ambient_temperature_degC();
|
|
const uint32_t now_us = AP_HAL::micros();
|
|
const uint32_t delta_t_us = now_us - last_servo_update_us;
|
|
if (delta_t_us < 1000) { // only update simulation at 1kHz
|
|
return;
|
|
}
|
|
last_servo_update_us = now_us;
|
|
const float dt = delta_t_us / 1000000.0;
|
|
|
|
for (auto &servo : servos) {
|
|
const uint8_t idx = servo.id - 1;
|
|
if (idx > ARRAY_SIZE(aircraft.servo_filter)) {
|
|
continue;
|
|
}
|
|
servo.position = aircraft.servo_filter[idx].angle();
|
|
|
|
// update primary current - proportional to airspeed and aileron deflection:
|
|
float airspeed = aircraft.get_airspeed_pitot();
|
|
servo.primary_current = abs(sinf(radians(servo.position))) * airspeed * 10;
|
|
|
|
// update temperatures
|
|
// servo.pcb_temperature += servo.secondary_current *0.01 * dt;
|
|
servo.pcb_temperature -= (servo.pcb_temperature-ambient_degC) * 0.1 * dt;
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
log_Servo(servo, now_us);
|
|
#endif // HAL_LOGGING_ENABLED
|
|
}
|
|
}
|
|
|
|
void Volz::update(const class Aircraft &aircraft)
|
|
{
|
|
if (!_enabled.get()) {
|
|
return;
|
|
}
|
|
|
|
update_servos(aircraft);
|
|
|
|
update_input();
|
|
}
|
|
|
|
void Volz::consume_command()
|
|
{
|
|
if (buflen < sizeof(Command)) {
|
|
AP_HAL::panic("No command to consume");
|
|
}
|
|
if (buflen > sizeof(Command)) {
|
|
memmove(&u.buffer[0], &u.buffer[sizeof(Command)], buflen-sizeof(Command));
|
|
}
|
|
buflen -= sizeof(Command);
|
|
}
|
|
|
|
bool Volz::shift_command_to_front_of_buffer()
|
|
{
|
|
while (buflen >= sizeof(Command)) {
|
|
if (u.command.calculate_checksum() == be16toh(u.command.crc)) {
|
|
return true;
|
|
}
|
|
// AP_HAL::panic("Invalid input");
|
|
|
|
// sync error; just shift everything by 1...
|
|
memmove(&u.buffer[0], &u.buffer[1], buflen-1);
|
|
buflen--;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Volz::process_command(const Command &command)
|
|
{
|
|
if (command.actuator_id >= ARRAY_SIZE(servos)+1) {
|
|
AP_HAL::panic("Invalid actuator ID (%u) received", command.actuator_id);
|
|
}
|
|
Servo &servo = servos[command.actuator_id-1];
|
|
|
|
Command response {};
|
|
response.actuator_id = command.actuator_id;
|
|
|
|
switch (command.command_id) {
|
|
case CommandId::SET_EXTENDED_POSITION: {
|
|
servo.desired_position = linear_interpolate(-1, 1, UINT16_VALUE(command.arg1, command.arg2), 0x80, 0x0F80);
|
|
response.command_id = CommandId::EXTENDED_POSITION_RESPONSE;
|
|
const uint16_t scaled_position = linear_interpolate(0x80, 0x0F80, servo.position, -1, 1);
|
|
response.arg1 = HIGHBYTE(scaled_position);
|
|
response.arg2 = LOWBYTE(scaled_position);
|
|
break;
|
|
}
|
|
case CommandId::READ_TEMPERATURE:
|
|
// nb. MAX(50) may not be correct.... unknown how true device performs
|
|
response.command_id = CommandId::TEMPERATURE_RESPONSE;
|
|
response.arg1 = MAX(servo.motor_temperature, -50.0) + 50.0;
|
|
response.arg2 = MAX(servo.pcb_temperature, -50.0) + 50.0;
|
|
break;
|
|
case CommandId::READ_CURRENT:
|
|
response.command_id = CommandId::CURRENT_RESPONSE;
|
|
response.arg1 = MIN(servo.primary_current * 50, 255); // realistic?
|
|
response.arg2 = MIN(servo.secondary_current * 50, 255);
|
|
break;
|
|
case CommandId::READ_VOLTAGE:
|
|
response.command_id = CommandId::VOLTAGE_RESPONSE;
|
|
response.arg1 = MIN(servo.primary_voltage * 5, 255);
|
|
response.arg2 = MIN(servo.secondary_voltage * 5, 255);
|
|
break;
|
|
case CommandId::CURRENT_RESPONSE:
|
|
case CommandId::VOLTAGE_RESPONSE:
|
|
case CommandId::TEMPERATURE_RESPONSE:
|
|
case CommandId::EXTENDED_POSITION_RESPONSE:
|
|
AP_HAL::panic("Response 'command' %u received", (unsigned)command.command_id);
|
|
break;
|
|
default:
|
|
AP_HAL::panic("Unknown command %u received", (unsigned)command.command_id);
|
|
}
|
|
|
|
response.update_checksum();
|
|
|
|
if (write_to_autopilot((char*)&response, sizeof(response)) != sizeof(response)) {
|
|
AP_HAL::panic("short write");
|
|
}
|
|
}
|
|
|
|
void Volz::update_input()
|
|
{
|
|
const ssize_t n = read_from_autopilot((char*)&u.buffer[buflen], ARRAY_SIZE(u.buffer) - buflen);
|
|
if (n < 0) {
|
|
// TODO: do better here
|
|
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
|
|
AP_HAL::panic("Failed to read from autopilot");
|
|
}
|
|
} else {
|
|
buflen += n;
|
|
}
|
|
|
|
while (shift_command_to_front_of_buffer()) {
|
|
process_command(u.command);
|
|
consume_command();
|
|
}
|
|
}
|
|
|
|
void Volz::update_sitl_input_pwm(struct sitl_input &input)
|
|
{
|
|
// overwrite the SITL input values passed through from
|
|
// sitl_model->update_model with those we're receiving serially
|
|
for (auto &servo : servos) {
|
|
const uint8_t idx = servo.id - 1;
|
|
if (idx > ARRAY_SIZE(input.servos)) {
|
|
// silently ignore; input.servos is 12-long, we are
|
|
// usually 16-long
|
|
continue;
|
|
}
|
|
if ((_output_mask.get() & (1U<<idx)) == 0) {
|
|
continue;
|
|
}
|
|
servo.failed = _failed_mask.get() & (1U<<idx);
|
|
input.servos[idx] = servo.pwm();
|
|
}
|
|
}
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
void Volz::log_Servo(const Volz::Servo &servo, const uint32_t now_us)
|
|
{
|
|
AP::logger().WriteStreaming(
|
|
"SMVZ",
|
|
"TimeUS," "Id," "Pos," "DesPos," "V," "A," "PCBT," "MotT" ,
|
|
"s" "#" "%" "%" "v" "A" "O" "O" ,
|
|
"F" "-" "0" "0" "0" "0" "0" "0" ,
|
|
"Q" "B" "f" "f" "f" "f" "f" "f" ,
|
|
now_us,
|
|
servo.id - 1,
|
|
servo.position * 100,
|
|
servo.desired_position * 100,
|
|
servo.primary_voltage,
|
|
servo.primary_current,
|
|
servo.pcb_temperature,
|
|
servo.motor_temperature
|
|
);
|
|
}
|
|
#endif // HAL_LOGGING_ENABLED
|
|
|
|
#endif // AP_SIM_VOLZ_ENABLED
|