aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: support for steering/throttle rover

Browse Source
This commit is contained in:
ashvath 2020-02-02 15:51:29 +05:30 committed by Andrew Tridgell
parent 7c5f67227b
commit 71f8c7cabb
2 changed files with 53 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/SITL/SIM_Morse.cpp
View File

@ -105,14 +105,16 @@ Morse::Morse(const char *frame_str) :
}
if (strstr(frame_option, "-rover")) {
output_type = OUTPUT_ROVER;
output_type = OUTPUT_ROVER_REGULAR;
} else if (strstr(frame_option, "-skid")) {
output_type = OUTPUT_ROVER_SKID;
} else if (strstr(frame_option, "-quad")) {
output_type = OUTPUT_QUAD;
} else if (strstr(frame_option, "-pwm")) {
output_type = OUTPUT_PWM;
} else {
// default to rover
output_type = OUTPUT_ROVER;
output_type = OUTPUT_ROVER_REGULAR;
}
for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
@ -339,10 +341,46 @@ bool Morse::sensors_receive(void)
return parse_ok;
}
/*
output control command assuming steering/throttle rover
*/
void Morse::output_rover_regular(const struct sitl_input &input)
{
float throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
float ground_steer = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
float max_steer = radians(60);
float max_speed = 20;
float max_accel = 20;
// speed in m/s in body frame
Vector3f velocity_body = dcm.transposed() * velocity_ef;
// speed along x axis, +ve is forward
float speed = velocity_body.x;
// target speed with current throttle
float target_speed = throttle * max_speed;
// linear acceleration in m/s/s - very crude model
float accel = max_accel * (target_speed - speed) / max_speed;
//force directly proportion to acceleration
float force = accel;
float steer = ground_steer * max_steer;
// construct a JSON packet for steer/force
char buf[60];
snprintf(buf, sizeof(buf)-1, "{\"steer\": %.3f, \"force\": %.2f, \"brake\": %.2f}\n",
steer, -force, 0.0);
buf[sizeof(buf)-1] = 0;
control_sock->send(buf, strlen(buf));
}
/*
output control command assuming skid-steering rover
*/
void Morse::output_rover(const struct sitl_input &input)
void Morse::output_rover_skid(const struct sitl_input &input)
{
float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
@ -510,8 +548,11 @@ void Morse::update(const struct sitl_input &input)
update_mag_field_bf();
switch (output_type) {
case OUTPUT_ROVER:
output_rover(input);
case OUTPUT_ROVER_REGULAR:
output_rover_regular(input);
break;
case OUTPUT_ROVER_SKID:
output_rover_skid(input);
break;
case OUTPUT_QUAD:
output_quad(input);
@ -626,4 +667,4 @@ void Morse::send_report(void)
}
}
}
}

10
libraries/SITL/SIM_Morse.h
View File

@ -64,15 +64,17 @@ private:
uint16_t morse_control_port = 60001;
enum {
OUTPUT_ROVER=1,
OUTPUT_QUAD=2,
OUTPUT_PWM=3
OUTPUT_ROVER_REGULAR=1,
OUTPUT_ROVER_SKID=2,
OUTPUT_QUAD=3,
OUTPUT_PWM=4
} output_type;
bool connect_sockets(void);
bool parse_sensors(const char *json);
bool sensors_receive(void);
void output_rover(const struct sitl_input &input);
void output_rover_regular(const struct sitl_input &input);
void output_rover_skid(const struct sitl_input &input);
void output_quad(const struct sitl_input &input);
void output_pwm(const struct sitl_input &input);
void report_FPS();