static void cmdIr(BaseSequentialStream *chp, int argc, char *argv[])
{
(void)argv;
(void)argc;
chEvtRegisterMask(&esSensorEvents , &el, SENSOR_EVENT_IR_END);
chprintf(chp, "Pinging with IR...\r\n");
chEvtBroadcastFlags(&esSensorEvents, SENSOR_EVENT_IR_START);
chEvtWaitOne(SENSOR_EVENT_IR_END);
chThdSleepMilliseconds(50);
ir_index_t i;
ir_angle_t angle;
chSysLock();
for (i = 0; i < IR_RX_COUNT; i++) {
angle = irDetections[i];
chprintf(chp, "IR Angle: %D\r\n", angle);
}
chSysUnlock();
chEvtUnregister(&esSensorEvents, &el);
}
/* прием и обработка комманд с земли*/
void process_cmd(mavlink_command_long_t *mavlink_command_long_struct){
/* all this flags defined in MAV_CMD enum */
switch(mavlink_command_long_struct->command){
case MAV_CMD_DO_SET_MODE:
/* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */
mavlink_system_struct.mode = mavlink_command_long_struct->param1;
command_accepted();
break;
/*
* (пере)запуск калибровки
*/
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
command_denied();
return;
}
else{
handle_calibration_cmd(mavlink_command_long_struct);
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
/* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
command_denied();
return;
}
else{
chEvtBroadcastFlags(&init_event, EVENT_MASK(SIGHALT_EVID));
command_accepted();
}
break;
/**
* Команды для загрузки/вычитки параметров из EEPROM
*/
case MAV_CMD_PREFLIGHT_STORAGE:
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT)
return;
if (mavlink_command_long_struct->param1 == 0)
load_params_from_eeprom();
else if (mavlink_command_long_struct->param1 == 1)
save_params_to_eeprom();
if (mavlink_command_long_struct->param2 == 0)
load_mission_from_eeprom();
else if (mavlink_command_long_struct->param2 == 1)
save_mission_to_eeprom();
break;
default:
return;
break;
}
}
static void send_ack(uint8_t type){
/* logically the target_component must be MAV_COMP_ID_MISSIONPLANNER,
* but QGC does not accept them. */
mavlink_out_mission_ack_struct.target_component = MAV_COMP_ID_ALL;
//mavlink_out_mission_ack_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_ack_struct.target_system = GROUND_STATION_ID;
mavlink_out_mission_ack_struct.type = type;
chEvtBroadcastFlags(&event_mavlink_out_mission_ack, EVMSK_MAVLINK_OUT_MISSION_ACK);
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
chEvtInit(&eventImuIrq);
chEvtInit(&eventMagnIrq);
chEvtInit(&eventImuRead);
chEvtInit(&eventMagnRead);
chEvtInit(&eventEKFDone);
palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );
I2CInitLocal();
configInit();
mavlinkInit();
initSensors();
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_Cmd(TIM5, ENABLE);
startEstimation();
startSensors();
radioInit();
motorsInit();
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, 0);
extChannelEnable(&EXTD1, 1);
chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);
while (TRUE) {
chThdSleepMilliseconds(1000);
}
}
void motor_beep(int frequency, int duration_msec)
{
chMtxLock(&_mutex);
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
_state.beep_frequency = frequency;
_state.beep_duration_msec = duration_msec;
chMtxUnlock();
chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS); // Wake the control thread
} else {
chMtxUnlock();
}
}
static msg_t PollIMUThread(void *arg){
(void)arg;
chRegSetThreadName("PollIMU");
struct EventListener self_el;
chEvtRegister(&imu_event, &self_el, 2);
while (TRUE) {
chEvtWaitAll(EVENT_MASK(0) && EVENT_MASK(4));
mpu_i2c_read_data(0x3B, 14); // Read accelerometer, temperature and gyro data
chEvtBroadcastFlags(&imu_event, EVENT_MASK(2));
}
return 0;
}
static msg_t MmcReaderDmThread(void *sdp){
chRegSetThreadName("MmcReaderDm");
struct EventListener el_storage_request_count_dm;
struct EventListener el_storage_request_dm;
chEvtRegisterMask(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM);
chEvtRegisterMask(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM);
eventmask_t evt = 0;
while (!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM | EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM, MS2ST(50));
if (!mmcIsCardInserted(&MMCD1))
continue;
else{
switch (evt){
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM):
bnapStorageAcquire(&Storage);
mavlink_oblique_storage_count_struct.count = Storage.used;
bnapStorageRelease(&Storage);
chEvtBroadcastFlags(&event_mavlink_oblique_storage_count, EVMSK_MAVLINK_OBLIQUE_STORAGE_COUNT);
break;
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM):
_oblique_storage_request_handler_dm(sdp);
break;
default:
break;
}
}
}
chEvtUnregister(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm);
chEvtUnregister(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm);
return 0;
}
void motor_set_duty_cycle(float dc, int ttl_ms)
{
chMtxLock(&_mutex);
_state.mode = MOTOR_CONTROL_MODE_OPENLOOP;
if (dc < 0.0) { dc = 0.0; }
if (dc > 1.0) { dc = 1.0; }
_state.dc_openloop_setpoint = dc;
_state.setpoint_ttl_ms = ttl_ms;
if (dc == 0.0) {
_state.num_unexpected_stops = 0;
}
chMtxUnlock();
// Wake the control thread to process the new setpoint immediately
chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS);
}
void motor_set_rpm(unsigned rpm, int ttl_ms)
{
chMtxLock(&_mutex);
_state.mode = MOTOR_CONTROL_MODE_RPM;
if (rpm > _params.rpm_max) {
rpm = _params.rpm_max;
}
_state.rpm_setpoint = rpm;
_state.setpoint_ttl_ms = ttl_ms;
if (rpm == 0) {
_state.num_unexpected_stops = 0;
}
chMtxUnlock();
// Wake the control thread to process the new setpoint immediately
chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS);
}
/**
* @details When the last waypoint was successfully received
* the requesting component sends a WAYPOINT_ACK message
* to the targeted component. This finishes the transaction.
* Notice that the targeted component has to listen to
* WAYPOINT_REQUEST messages of the last waypoint until it
* gets the WAYPOINT_ACK or another message that starts
* a different transaction or a timeout happens.
*/
static bool mav2gcs(void){
eventmask_t evt = 0;
bool status = CH_FAILED;
uint32_t retry_cnt = PLANNER_RETRY_CNT;
struct EventListener el_mission_request;
struct EventListener el_mission_ack;
struct EventListener el_mission_request_list;
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
chEvtRegisterMask(&event_mavlink_in_mission_request, &el_mission_request, EVMSK_MAVLINK_IN_MISSION_REQUEST);
chEvtRegisterMask(&event_mavlink_in_mission_ack, &el_mission_ack, EVMSK_MAVLINK_IN_MISSION_ACK);
START:
mavlink_out_mission_count_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_count_struct.target_system = GROUND_STATION_ID;
mavlink_out_mission_count_struct.count = wpdb.getCount();
chEvtBroadcastFlags(&event_mavlink_out_mission_count, EVMSK_MAVLINK_OUT_MISSION_COUNT);
if (0 == mavlink_out_mission_count_struct.count){
status = CH_SUCCESS;
goto EXIT;
}
/* теперь нам надо понять, дошло сообщение с количеством вейпоинтов, или нет.
* Если нет - земля пришле повторный запрос MISSION_REQUEST_LIST */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
EVMSK_MAVLINK_IN_MISSION_REQUEST |
EVMSK_MAVLINK_IN_MISSION_ACK,
PLANNER_RETRY_TMO * PLANNER_RETRY_CNT);
switch(evt){
case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
/* надобно повторить */
goto START;
/* maint loooong loop */
case EVMSK_MAVLINK_IN_MISSION_REQUEST:
do{
wpdb.load(&mavlink_out_mission_item_struct, mavlink_in_mission_request_struct.seq);
mavlink_out_mission_item_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_item_struct.target_system = GROUND_STATION_ID;
chEvtBroadcastFlags(&event_mavlink_out_mission_item, EVMSK_MAVLINK_OUT_MISSION_ITEM);
/* wait next request or ack */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST |
EVMSK_MAVLINK_IN_MISSION_ACK,
PLANNER_RETRY_TMO);
switch(evt){
case EVMSK_MAVLINK_IN_MISSION_REQUEST:
continue;
case EVMSK_MAVLINK_IN_MISSION_ACK:
status = CH_SUCCESS;
goto EXIT;
break;
default:
retry_cnt--;
status = CH_FAILED;
break;
}
}while(retry_cnt);
break; /* case EVMSK_MAVLINK_IN_MISSION_REQUEST */
case EVMSK_MAVLINK_IN_MISSION_ACK:
status = CH_SUCCESS;
break;
default:
status = CH_FAILED;
break;
}
EXIT:
chEvtUnregister(&event_mavlink_in_mission_request, &el_mission_request);
chEvtUnregister(&event_mavlink_in_mission_ack, &el_mission_ack);
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
return status;
}
static bool gcs2mav(uint16_t N){
uint32_t seq = 0;
uint32_t retry_cnt = PLANNER_RETRY_CNT;
eventmask_t evt = 0;
mavlink_mission_item_t mi; /* local copy */
uint8_t status = MAV_MISSION_ERROR;
/* check available space */
if (N > wpdb.getCapacity()){
send_ack(MAV_MISSION_NO_SPACE);
return CH_FAILED;
}
/* prepare to harvest waypoints */
struct EventListener el_mission_item;
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, 1);/* fake wait to clear "queue" */
if (CH_FAILED == wpdb.clear())
chDbgPanic("");
for (seq=0; seq<N; seq++){
/* prepare request to ground */
mavlink_out_mission_request_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_request_struct.target_system = GROUND_STATION_ID;
mavlink_out_mission_request_struct.seq = seq;
retry_cnt = PLANNER_RETRY_CNT;
chThdSleep(PLANNER_ADDITIONAL_TMO);
do{
/* drop message */
chEvtBroadcastFlags(&event_mavlink_out_mission_request, EVMSK_MAVLINK_OUT_MISSION_REQUEST);
/* wait answer */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, PLANNER_RETRY_TMO);
if (EVMSK_MAVLINK_IN_MISSION_ITEM == evt){
chSysLock();
mi = mavlink_in_mission_item_struct;
chSysUnlock();
/* check waypoint cosherness and write it if cosher */
status = check_wp(&mi, seq);
if (status != MAV_MISSION_ACCEPTED)
goto EXIT;
else{
if (CH_FAILED == wpdb.save(&mi, seq))
chDbgPanic("");
break; /* do-while */
}
}
retry_cnt--;
if(0 == retry_cnt)
goto EXIT;
}while(retry_cnt);
}
/* save waypoint count in eeprom only in the very end of transaction */
if (CH_FAILED == wpdb.finalize())
chDbgPanic("");
/* final stuff */
EXIT:
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
send_ack(status);
if (0 == retry_cnt)
return CH_FAILED;
else
return CH_SUCCESS;
}
void EvtSource::broadcastFlags(flagsmask_t flags) {
chEvtBroadcastFlags(&ev_source, flags);
}
void CcUnpackCycle(SerialDriver *sdp){
mavlink_message_t msg;
mavlink_status_t status;
msg_t c = 0;
msg_t prev_c = 0;
while (!chThdShouldTerminate()) {
if (!cc_port_ready()){
chThdSleepMilliseconds(50);
continue;
}
else{
/* Try to get an escaped with DLE symbols message */
c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
if (c == Q_TIMEOUT)
continue;
prev_c = c;
if (prev_c == DLE){
prev_c = 0; /* set it to any just not DLE nor ETX */
c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
if (c == Q_TIMEOUT)
continue;
}
}
if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)c, &msg, &status)) {
switch(msg.msgid){
case MAVLINK_MSG_ID_COMMAND_LONG:
mavlink_msg_command_long_decode(&msg, &mavlink_command_long_struct);
if (mavlink_command_long_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_command_long, EVMSK_MAVLINK_COMMAND_LONG);
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
mavlink_msg_param_request_read_decode(&msg, &mavlink_param_request_read_struct);
if (mavlink_param_request_read_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_request_read, EVMSK_MAVLINK_PARAM_REQUEST_READ);
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mavlink_msg_param_request_list_decode(&msg, &mavlink_param_request_list_struct);
if (mavlink_param_request_list_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_request_list, EVMSK_MAVLINK_PARAM_REQUEST_LIST);
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &mavlink_param_set_struct);
if (mavlink_param_set_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_set, EVMSK_MAVLINK_PARAM_SET);
break;
case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_CC:
mavlink_msg_oblique_storage_request_cc_decode(&msg, &mavlink_oblique_storage_request_cc_struct);
if (mavlink_oblique_storage_request_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_CC);
break;
case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_COUNT_CC:
mavlink_msg_oblique_storage_request_count_cc_decode(&msg, &mavlink_oblique_storage_request_count_cc_struct);
if (mavlink_oblique_storage_request_count_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_count_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_CC);
break;
case MAVLINK_MSG_ID_HEARTBEAT_CC:
mavlink_msg_heartbeat_cc_decode(&msg, &mavlink_heartbeat_cc_struct);
// if (mavlink_heartbeat_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_heartbeat_cc, EVMSK_MAVLINK_HEARTBEAT_CC);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
mavlink_msg_statustext_decode(&msg, &mavlink_statustext_struct);
// if (mavlink_statustext_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_statustext, EVMSK_MAVLINK_STATUSTEXT);
break;
case MAVLINK_MSG_ID_OBLIQUE_AGPS:
mavlink_msg_oblique_agps_decode(&msg, &mavlink_oblique_agps_struct);
if (mavlink_oblique_agps_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_agps, EVMSK_MAVLINK_OBLIQUE_AGPS);
break;
default:
break;
}
}
}
}
/**
* @brief Main estimation thread.
*
* @param[in/out] arg Unused.
*/
static THD_FUNCTION(ThreadEstimation, arg)
{
(void)arg;
chRegSetThreadName("Estimation");
tp = chThdGetSelfX();
/* Initialization data */
//uint32_t i;
//quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
//vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;
/* Event registration for new sensor data */
event_listener_t el;
/* Register to new data from accelerometer and gyroscope */
chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
&el,
EVENT_MASK(0),
ACCGYRO_DATA_AVAILABLE_EVENTMASK |
MAG_DATA_AVAILABLE_EVENTMASK |
BARO_DATA_AVAILABLE_EVENTMASK);
/* Force an initialization of the estimation */
//chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
vInitializeMotionCaptureEstimator(&states);
while(1)
{
//if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
// isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
// isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
// AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
/* Check if there has been a request to reset the filter */
//if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
// {
/* Initialize the estimation */
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
//}
/* Wait for new measurement data */
chEvtWaitAny(ALL_EVENTS);
eventflags_t flags = chEvtGetAndClearFlags(&el);
if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
{
/* Get sensor data */
GetIMUData(&imu_data);
/* Run estimation */
vInnovateMotionCaptureEstimator(&states,
&imu_data,
SENSOR_ACCGYRO_DT,
0.0007f);
/*InnovateAttitudeEKF(&states,
&data,
imu_data.gyroscope,
imu_data.accelerometer,
imu_data.magnetometer,
0.0f,
0.0f,
ESTIMATION_DT);*/
//states.w.x = -imu_data.gyroscope[0];
//states.w.y = -imu_data.gyroscope[1];
//states.w.z = imu_data.gyroscope[2];
//am.x = -imu_data.accelerometer[0];
//am.y = -imu_data.accelerometer[1];
//am.z = imu_data.accelerometer[2];
//states.q = MadgwickAHRSupdateIMU(states.w,
// am,
// states.q,
// 0.15f,
// dt);
/* Broadcast new estimation available */
chEvtBroadcastFlags(&estimation_events_es,
ESTIMATION_NEW_ESTIMATION_EVENTMASK);
}
}
}
void EvtSource::broadcastFlags(eventflags_t flags) {
chEvtBroadcastFlags(&ev_source, flags);
}