/**
* The structure of the entry point function for one of
* the 3 'output' boards. The sensor board will differ slightly in
* its running state.
*/
void main(void) {
// initialise stuff
proto_init();
while (1) {
switch (proto_state()) {
case STARTUP:
break;
case CALIBRATING:
break;
case RUNNING:
if (proto_msg_buff_length()) {
msg = proto_msg_buff_pop();
if (msg.command == CMD_SET) {
set_steering(msg.data[0]);
proto_refresh();
} else {
proto_state_error();
}
}
break;
default: // ERROR
//broadcast ERROR signal
break;
}
}
}
void ModeAcro::update()
{
// get speed forward
float speed, desired_steering;
if (!attitude_control.get_forward_speed(speed)) {
float desired_throttle;
// convert pilot stick input into desired steering and throttle
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// no valid speed, just use the provided throttle
g2.motors.set_throttle(desired_throttle);
} else {
float desired_speed;
// convert pilot stick input into desired steering and speed
get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
calc_throttle(desired_speed, true);
}
float steering_out;
// handle sailboats
if (!is_zero(desired_steering)) {
// steering input return control to user
rover.sailboat_clear_tack();
}
if (g2.motors.has_sail() && rover.sailboat_tacking()) {
// call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
g2.wp_nav.get_pivot_rate(),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
} else {
// convert pilot steering input to desired turn rate in radians/sec
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// run steering turn rate controller and throttle controller
steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
}
set_steering(steering_out * 4500.0f);
}
static void httpd_websocket_cb(struct altcp_pcb *pcb, uint8_t *data,
uint16_t data_len, uint8_t mode)
{
if (data_len == 0 || mode != WS_BIN_MODE) return;
uint8_t msgtype = data[0];
uint8_t *payload = &data[1];
data_len -= 1;
int8_t *signed_data;
pid_controller_index pid_index;
int32_t *i32_data;
uint8_t res;
switch (msgtype)
{
case STEERING:
// Parameters: velocity (int8_t), turn rate (int8_t)
if (data_len != 2) break;
signed_data = (int8_t *)payload;
set_steering((FLT_TO_Q16(SPEED_CONTROL_FACTOR) * signed_data[1]) / 128,
(FLT_TO_Q16(STEERING_FACTOR) * signed_data[0]) / 128);
break;
case REQ_ORIENTATION:
if (data_len != 0) break;
send_orientation(pcb);
break;
case REQ_SET_PID_PARAMS:
// Parameters: pid index (uint8_t), P (q16/int32_t), I (q16), D (q16)
if (data_len != 13) break;
pid_index = (pid_controller_index)payload[0];
i32_data = (int32_t *)(&payload[1]);
if (pid_index < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
{
update_pid_controller(pid_index, i32_data[0], i32_data[1],
i32_data[2]);
res = RES_SET_PID_PARAMS_ACK;
httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
}
break;
case REQ_GET_PID_PARAMS:
if (data_len != 1) break;
if (payload[0] < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
{
send_pid_params(pcb, (pid_controller_index)payload[0]);
}
break;
case REQ_LOAD_FLASH_CONFIG:
if (data_len != 0) break;
load_config();
res = RES_LOAD_FLASH_CONFIG_DONE;
httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
break;
case REQ_SAVE_CONFIG:
if (data_len != 0) break;
httpd_websocket_save_config(pcb);
break;
case REQ_CLEAR_CONFIG:
if (data_len != 0) break;
httpd_websocket_clear_config(pcb);
break;
}
}