void Arbitrator::start() {
bool running = true;
RobotAction::Control control;
while(running) {
// Read current values from sensors
// Actions that need access to the data, whould have been passed
// a reference to it in their constructor
update_sensor_data();
assess_actions(control);
}
return;
}
void tick_task(const uint8_t tick_type) {
static int8_t message_counter = 0;
if(tick_type == TICK_TASK_TYPE_CALCULATION) {
update_sensor_data();
if(update_sensor_counter == 5) {
imu_blinkenlights();
}
for(uint8_t i = 0; i < IMU_PERIOD_NUM; i++) {
if(imu_period_counter[i] < UINT32_MAX) {
imu_period_counter[i]++;
}
}
} else if(tick_type == TICK_TASK_TYPE_MESSAGE) {
if(usb_first_connection && !usbd_hal_is_disabled(IN_EP)) {
message_counter++;
if(message_counter >= 100) {
message_counter = 0;
if(brick_init_enumeration(COM_USB)) {
com_info.current = COM_USB;
usb_first_connection = false;
message_counter = 0;
}
}
}
for(uint8_t i = 0; i < IMU_PERIOD_NUM; i++) {
if((imu_period[i] != 0) &&
(imu_period[i] <= imu_period_counter[i])) {
// Test if we are totally out of time (lost a whole
// period), in this case we don't send the callback again.
// This is needed for the wireless extensions
if(imu_period[i]*2 <= imu_period_counter[i]) {
imu_period_counter[i] = imu_period[i];
}
make_period_callback(i);
}
}
}
}
void FlightDynamics::task(void){
//Download Sensor data from sensors:
update_sensor_data();
//Run extended kalman filter:
if(raw_acc[0] != 0 && raw_acc[1] != 0 && raw_acc[2] != 0){
EKF.innovate_priori(attitude_quaternion, raw_gyro[0], raw_gyro[1], raw_gyro[2]);
//EKF.innovate_priori(attitude_quaternion, 0, 0, 0);
EKF.innovate_stage1(attitude_quaternion, raw_acc[0], raw_acc[1], raw_acc[2]);
attitude_quaternion.normalize();
}
attitude_euler = attitude_quaternion.to_euler();
attitude_euler.to_degrees();
//Update the attitude (euler) interface/socket:
attitude_socket.pitch = attitude_euler.x*-1;
attitude_socket.roll = attitude_euler.y;
attitude_socket.yaw = attitude_euler.z*-1;
attitude_socket.pitch_velocity = pitch_filter.process(raw_gyro[0])*-1;
attitude_socket.roll_velocity = roll_filter.process(raw_gyro[1]);
attitude_socket.yaw_velocity = yaw_filter.process(raw_gyro[2]*-1);
//Update the quarternion interface/socket:
attitude_quarternion_socket = attitude_quaternion;
//Publish the data to other stake-holders:
attitude_socket.publish();
attitude_quarternion_socket.publish();
check_calibrations();
update_status();
debug_led();
if(debugging_stream) handle_debug_stream();
}
static void
sweep (sonar_configuration *sp)
{
sonar_bogie *b;
/* Move the sweep forward (clockwise).
*/
GLfloat prev_sweep, this_sweep, tick;
GLfloat cycle_secs = 30 / speed; /* default to one cycle every N seconds */
this_sweep = ((cycle_secs - fmod (double_time() - sp->start_time +
sp->sweep_offset,
cycle_secs))
/ cycle_secs
* M_PI * 2);
prev_sweep = sp->sweep_th;
tick = prev_sweep - this_sweep;
while (tick < 0) tick += M_PI*2;
sp->sweep_th = this_sweep;
if (this_sweep < 0 || this_sweep >= M_PI*2) abort();
if (prev_sweep < 0) /* skip first time */
return;
if (tick < 0 || tick >= M_PI*2) abort();
/* Go through the 'pending' sensor data, find those bogies who are
just now being swept, and move them from 'pending' to 'displayed'.
(Leave bogies that have not yet been swept alone: we'll get to
them when the sweep moves forward.)
*/
b = sp->pending;
while (b)
{
sonar_bogie *next = b->next;
if (point_in_wedge (b->th, this_sweep, prev_sweep))
{
if (debug_p > 1) {
time_t t = time((time_t *) 0);
fprintf (stderr,
"%s: sweep hit: %02d:%02d: %s: (%5.2f %5.2f %5.2f;"
" th=[%.2f < %.2f <= %.2f])\n",
progname,
(int) (t / 60) % 60, (int) t % 60,
b->name, b->r, b->th, b->ttl,
this_sweep, b->th, prev_sweep);
}
b->ttl = M_PI * 2.1;
copy_and_insert_bogie (sp->ssd, b, &sp->displayed);
delete_bogie (sp->ssd, b, &sp->pending);
}
b = next;
}
/* Update TTL on all currently-displayed bogies; delete the dead.
Request sensor updates on the ones just now being swept.
Any updates go into 'pending' and might not show up until
the next time the sweep comes around. This is to prevent
already-drawn bogies from jumping to a new position without
having faded out first.
*/
b = sp->displayed;
while (b)
{
sonar_bogie *next = b->next;
b->ttl -= tick;
if (b->ttl <= 0)
{
if (debug_p > 1)
fprintf (stderr, "%s: TTL expired: %s (%5.2f %5.2f %5.2f)\n",
progname, b->name, b->r, b->th, b->ttl);
delete_bogie (sp->ssd, b, &sp->displayed);
}
b = next;
}
update_sensor_data (sp);
}
